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ABSTRACT 


The  recognition  of  underwater  objects  and  obstacles  by  sonar  has  been  explored  in 
many  forms,  particularly  through  the  use  of  high-resolution  imaging  sonar  systems.  This 
work  explores  a  method  of  providing  real-time  obstacle  avoidance  and  navigational 
position  updating  for  an  Autonomous  Underwater  Vehicle  (AUV)  by  applying  regression 
analysis  and  geometric  interpretation  to  sonar  range  data  obtained  from  a  low-cost,  low- 
resolution,  fixed-beam  sonar.  The  algorithm  utilized  by  this  method  first  develops  a  least- 
squares  fit  for  sonar  range  data  in  a  2-D  manner.  The  parameters  developed  by  this  method 
are  then  compared  to  an  environmental  model  for  position  identification.  If  no  match  is 
achieved,  then  by  applying  the  known  geometry  of  the  acoustic  signal,  an  estimate  for  a  3- 
D  surface  is  derived.  This  derived  3-D  surface  is  then  added  to  the  environmental  model  to 
enable  accurate  path  planning  and  post-mission  analysis  information.  This  method  is 
currently  implemented  on  an  operational  AUV  operating  in  a  well-defined  onhogonal 
environment  at  NPS.  The  paper  also  discusses  the  simulation  of  the  sonar  systems  using  a 
ray  tracing  technique  in  a  real-time  dynamic  graphical  simulation  implemented  on  a  Silicon 
Graphics  IRIS  workstation. 
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1.  INTRODUCTION 


A.  MOTIVATION  FOR  AUV  RESEARCH 

Remotely  operated  air  and  land  vehicles  hav.  been  developed  to  extend  the  range  of 
sensors  or  weapons  of  military  units.  The  Israeli  Air  Force  utilized  remotely  piloted 
vehicles  (RPVs)  for  reconnaissance  and  electronic  intelligence  collection  prior  to  the 
successful  air  strikes  in  the  Bekka  Valley  in  1982.  This  utilization  of  RPVs  as  tactical 
probes  resulted  in  zero  losses  to  tlie  l.sraeli  Air  Force,  and  near-total  destruction  of  il.. 
Syrian  Air  Force  [Ref.  1]. 

Current  Maritime  Strategy  emphasizes  the  U.S.  Navy’s  role  in  a  forward  area  strategy 
conducting  operations  in  or  near  enemy  waters  [Ref.  2].  Given  the  Soviet  Navy’s  “bastion” 
defense  strategy,  utilizing  multiple  layers  of  defensive  units  to  protect  strategic  missile 
submarines,  an  underwater  vehicle  could  serve  as  an  effective  tactical  probe.  Due  to  the 
long-range  aspect  of  such  a  mission  and  the  presence  of  numerous  enemy  units,  the 
presence  of  a  “mother”  platform  providing  a  radio  control  link  is  not  feasible.  An 
autonomous  underwater  vehicle  (AUV)  with  onboard  sensors  and  control  units,  capable  of 
carrying  out  a  preplanned  mission  without  external  guidance,  has  been  shown  to  be  of 
potential  military  value  and  cost  effectivene.ss  [Ref.  1]. 

While  the  attention  given  to  Soviet  and  Warsaw  Pact  military  strategies  may  have 
lessened  due  to  the  decline  of  Communism  in  Eastern  Europe  and  the  Soviet  Union,  events 
in  and  around  the  Persian  Gulf  have  validated  the  need  for  truly  autonomous  vehicles  to 
assist  in  effecting  the  Maritime  Strategy. 
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Numerous  missions  are  conceivable  for  an  AU  V.  Some  typical  missions  might  include 
the  following: 

•  Tactical  probe 

•  Covert  surveillance 

•  Mine  warfare 

•  Weapons  delivery 

•  Underwater  terrain  mapping. 

Equipped  with  the  appropriate  sensors  and  manipulators,  an  AUV  could  carry  out  these 
missions  without  exposing  tactical  units  or  personnel  to  danger,  and  for  a  lower  cost. 

B.  AUV  CONCEPT 

In  the  simplest  fomi,  an  AUV  is  an  unmanned  submersible  vehicle  with  onboaiu 
systems  and  sub-systems  that  provide  motive  power,  motion  control,  navigation,  obstacle 
detection  and  collision  avoidance.  To  be  truly  autonomous,  the  vehicle  should  be  able  to 
execute  a  planned  mission  by  controlling  and  monitoring  the  onboard  systems  without  any 
external  input.  It  should  be  able  to  replan  its  mission  in  the  event  of  internal  anomalies,  such 
as  sub-system  degradation,  and  it  should  have  the  capability  of  replanning  its  path  to  avoid 
previously  unknown  obstacles  (Ref.  3J.  While  the  above  qualities  outline  the  minimum  re¬ 
quirements  for  a  simple  AUV,  a  realistic  mission-capable  AUV  would  necessarily  carry 
additional,  possibly  specialized,  mission-dependent  sub-systems  and  sensors.  The  Naval 
Postgraduate  School  (NPS)  AUV  II  is  designed  to  model  the  simple  AUV  concept. 

C.  OBSTACLE  AVOIDANCE 

The  concept  of  obstacle  avoidance  for  an  AUV  entails  more  than  simply  turning  the 
vehicle  in  a  predefined  manner  to  avoid  a  collision.  In  the  context  of  an  AUV  conducting 
a  predefined  mission,  the  concept  of  obstacle  avoidance  carries  the  implicit  notion  that  the 
AUV  will  attempt  to  continue  its  mission  by  replanning  its  path  around  the  obstacle.  This 
further  implies  two  additional  capabilities,  the  ability  to  consult  a  stored  moJtl  of  the  en¬ 
vironment,  and  the  ability  to  detect,  recognize,  and  quantify  previously  unknown  obstacles 
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so  that  they  may  be  added  to  the  stored  model  [Ref.  3].  This  can  be  simplified  into  the  fol¬ 
lowing  steps: 


•  Detect  the  obstacle. 

•  Perform  a  safety  maneuver  to  avoid  the  obstacle. 

•  Perform  other  maneuvers  to  extract  obstacle  features. 

•  Add  the  obstacle  to  the  stored  environmental  model. 

•  Replan  the  path  to  avoid  the  obstacle  while  continuing  the  mission. 


This  thesis  will  address  all  but  the  final  step.  Work  on  the  path  replanning  problem  has  been 
addressed  by  other  NPS  AUV  II  project  team  members  [Ref.  4], 

D.  OBJECTIVES 

This  thesis  will  address  the  following  research  questions: 

•  How  can  data  from  ultrasonic  sensors  be  best  utilized  to  recognize  obstacles 
during  operation  of  the  NPS  AUV  II? 

•  What  is  the  optimal  configuration  for  ultrasonic  sensors  on  the  AUV  II  to 
provide  obstacle  detection  and  terrain  data  collection  for  post-mission 
analysis? 

•  What  type  of  motion  algorithm  will  best  provide  collision  avoidance  and 
obstacle  feature  extraction? 

•  How  can  sensor  data  be  utilized  in  post-mission  analysis  to  generate  a  3-D 
terrain  model? 


This  research  was  designed  to  move  the  NPS  AUV  II  project  into  its  next  phase  of  devel¬ 
opment  by  providing  a  stable  test  platform  capable  of  maintaining  a  collision-free  path 
while  conducting  missions  in  its  current  environment,  the  NPS  swimming  pool.  At  the  out¬ 
set  of  this  research,  the  AUV  II  had  no  sensors  installed  and  was  capable  of  performing  only 
simple,  open-loop  missions  in  the  swimming  pool. 

E.  THESIS  ORGANIZATION 

Subsequent  chapters  of  this  work  will  address  the  research  questions  posed  above. 
Chapter  II  describes  the  NPS  AUV  II  in  terms  of  its  physical  characteristics  as  well  as  its 
control  and  software  hierarchy.  This  section  describes  previous  work  done  on  this  project. 
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Chapter  II  also  examines  the  most  current  work  on  sonars  and  AUVs.  Chapter  III  provides 
the  theoretical  framework  for  sonar  system  operation,  the  physical  and  electrical  character¬ 
istics  of  the  sonar  chosen  for  the  NPS  AU  V  II,  and  some  of  the  problems  inherent  with  these 
systems  and  their  installation. 

Chapter  IV  discusses  the  extraction  of  linear  features  of  obstacles  by  means  of  a  least 
squares  fit  algorithm.  The  algorithm  and  the  particular  tests  performed  on  sonar  range 
inputs  are  examined,  also.  Chapter  V  covers  the  utilization  of  the  information  developed  by 
linear  feature  extraction  by  the  collision  avoidance  system  and  other  processes. 
Experimental  results  are  presented  in  Chapter  VI.  Pre-mission  simulation  and  post-mission 
analysis  and  3-D  di.spla\  of  mi^.^ion  data  is  discussed  in  Chapier  \’II.  Finally,  conclusioi-.N 
regarding  the  viability  of  this  particular  approach  to  collision  avoidance  and 
recommendations  for  further  study  are  presented  in  Chapter  VIII. 
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II.  NPS  AUV  PROJECT  AND  RELATED  AUV  WORK 

The  Naval  Postgraduate  School’s  AUV  project  was  started  in  1987,  and  involves 
personnel  from  the  Mechanical  Engineering,  Computer  Science,  and  Electrical 
Engineering  departments.  The  project  has  evolved  through  design  and  feasibility  studies, 
the  construction  and  testing  of  the  NPS  AUV  I  radio-controlled  model  vehicle,  the 
construction  of  the  AUV  II  vehicle,  and  its  ongoing  testing  and  development.  [Refs.  5, 6, 7] 

A.  THE  NPS  AUV  II  VEHICLE 

1.  Vehicle  Characteristics 

The  basic  layout  of  equipment  for  the  AUV  II  vehicle  is  illustrated  in  Figure  2-1. 
The  vehicle  design  has  been  detailed  by  Good  [Ref.  8].  The  main  vehicle  body  is 
constructed  as  an  aluminum  box  with  a  beam  of  16  inches,  a  height  of  10  inches,  and  a 
length  of  72  inches.  The  nose  cone  is  constructed  of  fiberglass  and  extends  the  overall 
vehicle  length  to  92  inches.  The  vehicle  uses  fixed  ballast  and  displaces  approximately  390 
pounds.  Vehicle  control  is  provided  by  eight  independently  driven  control  surfaces,  four 
tunnel  thrusters,  and  two  main  drive  motors.  The  counter-rotating  24  volt  DC  drive  motors 
power  four-inch  propellers  and  provide  one-eighth  horsepower  each,  driving  the  AUV  at  a 
maximum  speed  of  about  two  knots  (3  feet  per  second).  The  control  surfaces  provide  a  high 
degree  of  maneuverability,  with  a  minimum  turning  diameter  of  approximately  20  ft.,  less 
than  three  ship  lengths.  All  of  the  installed  systems  are  powered  by  lead-acid  gel  batteries 
capable  of  providing  power  for  up  to  two  and  one-half  hours. 

The  control  and  guidance  software  processes  run  on  a  GESPAC  MPU  20HF  processor 
with  a  Motorola  68020  CPU  and  68881  math  coprocessor  running  at  16  MHz.  The  system 
has  2.5  Mb  of  RAM  and  runs  the  OS-9  multi-tasking  operating  system.  Input  and  output 
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between  the  CPU  and  the  installed  systems  is  routed  through  two  GESDAC-2B  8-channel  12  bit 
Digital-to-Analog/Analog-to-Digital  (DA/AD)  converter  cards  and  a  GESPIA-3A  parallel 
interface  board.  Serial  communications  with  external  systems  is  achieved  via  a  2400  bps  modem. 
The  navigation  system  sensor  suite  includes  a  flux  gate  compass  and  directional  gyroscope,  a 
vertical  gyroscope  system,  and  a  three  axis  rate  gyroscope  system  with  translational 
accelerometers.  A  paddlewheel  speed  sensor  is  installed  in  the  nose,  along  with  the  four  sonar 
transducers. 


Figure  2-1.  NPS  AUV  II  Vehicle  Component  Layout. 
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2.  Software  Hierarchy 

A  block  diagram  illustrating  the  interaction  of  systems  and  processes  is  given  in 
Figure  2-2  [Ref.  9].  The  dataflow  diagram  in  Figure  2-3  represents  the  software 
instantiation  of  the  processes  shown  in  Figure  2-2.  The  individual  data  items  are  further 
described  in  the  Data  Dictionary  located  in  Appendix  A.  The  mission  plan  and  the 
particular  environmental  database  for  the  operating  area  are  downloaded  to  the  AUV  from 
the  mission  support  system,  running  on  a  GRIDCASE  386  laptop  computer,  via  the 
modem.  Referring  now  to  Figure  2-2.  the  mission  executor  oversees  the  mission  exccutii^’'' 
by  providing  geographic  waypoints  and  tasks  to  the  guidance  system.  The  guidance  system 
provides  desired  vehicle  postures,  {x,y,  z,  6),  as  heading,  speed,  and  depth  commands  to 
the  autopilot  system.  The  autopilot,  updated  by  the  navigation  system,  controls  vehicle 
systems  to  achieve  the  desired  postures.  Vehicle  systems  are  monitored  for  possible 
problems,  such  as  the  loss  of  a  control  serx'o,  that  might  necessitate  mission  replanning. 

The  sonar  systems  provide  range  data  that  are  used  to  detect  obstacles  and  to 
develop  obstacle  features.  The  pattern  recognition  system  attempts  to  match  obstacle 
features  with  known  obstacles  in  the  database  in  order  to  provide  position  updates  to  the 
navigation  system.  Those  obstacles  that  are  not  found  in  the  database  are  added  to  the 
model,  and  the  obstacle  avoidance  decision  maker  is  signalled  to  take  possible  evasive 
measures.  Details  of  the  sonar  processes  are  discussed  in  Chapter  V.  Additionally,  the 
mission  replanner  is  signalled  to  develop  a  new  path  utilizing  the  updated  environmental 
database  model.  All  of  the  processes  running  on  the  GESPAC  are  coded  in  C. 
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B.  RELATED  WORK 


1.  Sonar  Range  Finding 

There  are  a  few  research  reports  on  linear  fitting  on  radial  range  data  from  ultrasonic 
sensors  in  autonomous  land  vehicle  control.  Crowley  proposed  the  recursive  linear  fitting 
algorithm  to  find  a  linear  segment  among  radial  data  [Ref.  10].  Drumheller  proposed  an 
iterative  endpoint  fit  for  the  same  purpose  [Ref.  11].  Several  reports  stress  the  uncertainty 
and  ambiguity  of  sonar  range  data  [Refs.  12,  13,  14].  The  method  of  range  data  fitting 
detailed  in  Chapter  IV  was  tested  on  the  autonomous  mobile  robot  Yamabico- 1 1  which  was 
developed  at  the  University  of  California  at  Santa  Barbara  and  at  the  Naval  Postgraduate 
School  (NPS)  by  Kanayama  [Ref.  15]. 

2.  AUV  Sonar  Research 

One  other  AUV  project  has  utilized  a  similar  sonar  system,  and  used  range  data 
slope  information  provided  by  the  sensors  to  assess  targets  [Ref.  1 6].  Most  work  in  this  area 
has  utilized  high  resolution  sector-scanning,  or  multi-beam  type  sonars  and  image 
processing  algorithms  for  obstacle  recognition  [Refs.  17,  18]  and  position  estimation  [Ref. 
19]. 
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III.  ULTRASONIC  TRANSDUCER  PROPERTIES 
AND  INSTALLATION 


A.  DATASONICS  PSA-900  PROGRAMMABLE  SONAR  ALTIMETER 
1.  Physical  and  Electrical  Characteristics 
The  ultrasonic  transducer  chosen  for  use  on  the  NPS  AUV  project  is  the  PSA-900 
Programmable  Sonar  Altimeter  manufactured  by  Datasonics,  Inc.  Each  transducer  system 
consists  of  a  transducer  head,  a  microprocessor- based  control  system,  and  associated 
connecting  cables.  Specifications  for  the  PSA-900  are  listed  in  Table  1.  Each  transducer 
head  measures  2.25  inches  in  diameter,  and  1  to  2  inches  in  length. 


TABLE  1-1. 

DATASONICS  PSA-900  SPECIFICATIONS 

Operating  Frequency; 

175/200/223  kHz  (fixed) 

Beam  Pattern; 

10°,  conical 

Pulse  Length: 

350  psecs 

Repetition  Rate; 

User-selectable  (10/  1  /O.l  pps) 

Range: 

User- selectable  (30  /  300  meters) 

Resolution: 

1  cm  @  30  m  range  /  10  cm  @  300  m  range 

Accuracy: 

±  0.25%  of  full  scale  range 

Range  Output; 

0  -  10  V  DC,  proportional  to  full  scale  range 

Power  Requirement; 

15-28v(2)  100  mA  DC 

[Ref.  20] 
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2.  Theory  of  Operation 

A  sonar  operates  by  emitting  acoustic  energy  at  a  specified  frequency  and  duration. 
Sonar  range  is  determined  by  timing  the  sound  pulse  as  it  travels  from  the  transducer, 
strikes  an  object,  and  then  returns  to  the  transducer.  If  the  speed  of  sound  through  the  water 
(Q)  is  known,  then  the  range  {d)  can  be  determined  from  the  following  formula: 


d  = 


(C,±AC,)  (r±Ar) 
2 


(EqS.l) 


The  nominal  speed  of  sound  (Q)  in  salt  water  with  salinity  of  35%o  is  1500  meters  per  sec¬ 
ond.  The  AC^  factor  represents  changes  in  sound  velocity  due  to  environmental  factors 
such  as  temperature,  pressure,  salinity  and  depth.  Of  these  factors,  temperature  change  con¬ 
tributes  the  most  to  sound  velocity  change,  with  as  much  as  a  five  percent  net  increase  or 
decrease  in  sound  velocity.  To  compensate  for  temperature  related  changes  in  sound  veloc¬ 
ity,  the  PSA-900  includes  a  temperature  sensor  that  enables  it  to  make  automatic 
adjustments  based  on  actual  temperature  readings.The  Ar  factor  in  (Eq  3. 1)  is  the  error  fac¬ 
tor  in  determining  the  actual  time  elapsed  since  the  sound  pulse  was  generated  and  is 
referred  to  as  jitter.  For  the  PSA-900  sonar  this  jitter  error  can  be  as  small  as  5  microsec¬ 
onds,  or  approximately  0.4  cm  total  distance  [Ref.  20]. 

The  ability  of  a  sonar  to  detect  an  echo  is  determined  by  the  initial  pulse  strength, 
the  size  and  type  of  the  target,  the  distance  to  the  target,  and  other  factors  related  to  noise. 
Expressed  in  terms  of  the  detection  threshold  (DT),  or  the  ability  of  the  sonar  to  just  detect 
a  target,  the  active  sonar  equation  is 

DT  =  SL-  2TL  +  TS-  (NL  -  DI)  (Eq  3.2) 

Here,  SL  is  the  signal  level  of  the  original  pulse,  2TL  is  the  two-way  transmission  path  loss, 
NL  is  the  background  noise,  and  Dl  is  the  directivity  index  of  the  receiver.  All  temis  are 
expressed  in  units  of  decibels  relative  to  the  standard  reference  intensity  of  a  IpPa  plane 
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wave.  The  transmission  loss  (JL)  is  due  to  spherical  spreading  of  the  sound  energy  and  ab¬ 
sorption  of  sound  energy  by  particles  suspended  in  the  water.  For  the  active  sonar  this  is 
expressed  as 

TL  =  201og2r-i-2ar  (Eq3.3) 

where  a  is  the  attenuation  coefficient  of  sound  in  water  at  the  frequency  in  use  and  r  is  the 
length  of  the  transmission  path  [Ref.  21,  pp.  19-23,  110-111].  With  the  AUV  operating  in 
the  swimming  pool  environment,  the  noise  term  {NL)  can  be  disregarded.  To  compensate 
for  the  transmission  loss  expressed  in  (Eq  3.3),  the  PSA-900  utilizes  a  time  var>'ing  gain 
(TVG)  amplifier  to  enhance  signal  detection  capability.  This  TVG  circuitry  increases  the 
gain  of  the  receiver  amplifier  as  a  function  of  time  to  ensure  that  weak  echoes  are  detected 
[Ref.  20]. 

3.  Side  Lobe  Effects 

The  PSA-900  transducer  is  a  circular  plane  type  and  produces  a  main  acoustic  beam 
lobe  that  extends  approximately  15  degrees  around  the  centerline,  producing  a  circular 
pattern.  Additionally,  the  transducer  produces  significant  side  lobes  at  approximately  25 
and  50  degrees  around  the  centerline.  Any  objects  in  the  area  of  the  side  lobes  can  produce 
echoes  as  though  the  object  were  along  the  centerline  of  the  sonar.  These  false  returns  can 
produce  erroneous  results  unless  corrected  or  eliminated. 

Since  it  is  impossible  to  alter  the  physical  characteristics  of  the  sonar  beam,  the 
method  of  attempting  to  eliminate  the  side  lobe  returns  focused  on  the  design  of  a  shield  to 
be  placed  on  each  transducer.  The  shield  is  in  the  form  of  a  conic  section,  measuring  2.5  in. 
in  length,  with  an  angle  of  5°  on  either  side  of  the  centerline.  This  shield  deflects  the  initial 
side  lobe  pattern  and  blocks  sonar  returns  outside  of  5°  of  the  sonar’s  extended  centerline. 
Figure  3-1  illustrates  the  effect  of  the  shield. 
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Figure  3-1.  2-D  \  icw  of  llie  Shielded  Sonar  Beam. 


4.  Interference  Problems 

Preliminary  investigations  using  these  sonars  revealed  that  simultaneous  operation 
of  two  sonar  systems,  both  operating  in  a  self-keying  mode  with  different  frequencies, 
could  result  in  interference  and  erroneous  range  readings.  Previous  tests  with  both  systems 
keyed  simultaneously  via  the  external  keying  signal  input  showed  that  mutual  interference 
should  not  be  a  problem  [Ref.  22,  pp.  28-33].  Post-mission  data  analysis  of  tests  conducted 
with  two  sonars,  mounted  orthogonally  in  the  AUV’s  nose  and  keyed  simultaneously, 
revealed  no  evidence  of  mutual  interference. 

5.  Sonar  Board  Averaging 

The  PSA-900  processor  maintains  a  sliding  window  average  consisting  of  the  last 
four  ranges.  This  average  is  used  to  determine  the  validity  of  range  signals,  with  signals 
differing  from  the  average  by  more  than  10%  of  the  maximum  selected  range  (3  meters) 
considered  to  be  in  error.  Due  to  this  on-board  averaging,  when  a  different  sonar  transducer 
is  selected  for  use  with  the  same  board,  a  number  of  pings  must  be  conducted  to  “wash  out” 
the  effects  of  the  previous  average.  Test  results  revealed  that  a  minimum  of  15  pings  must 
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be  generated  before  the  new  average  settled  at  the  correct  range  (Figure  3-2).  With  a  ping 
rate  of  10  Hz,  this  reduces  the  effective  data  acquisition  rate  to  less  than  0.3  Hz  for  each 
transducer  operating  on  the  same  board.  While  this  rate  is  acceptable  for  some  missions,  it 
is  too  low  for  purposes  of  control  or  obstacle  avoidance. 

6.  Noise  Filtering 

Initial  tests  conducted  with  the  sonars  installed  in  the  AUV  showed  that  the  signals 
provided  by  the  sonars  contained  a  noticeable  amount  of  spurious  noise.  Examination  of  a 
number  of  sets  of  test  data  led  to  the  observation  that  the  noise  tended  to  present  itself  as  a 
transient  spike  indicating  a  range  shorter  than  the  actual  range.  Further,  each  spike’s  first 
transition  duration  (rise  time)  was  very  short,  with  the  first  data  point  in  the  spike  being  13 
-  20  units  less  than  the  previous  valid  data  point.  The  .pikes  average  duration  was  1  second. 
At  a  10  Hz  data  rate,  this  means  that  ten  consecutive  data  points  may  be  invalid.  An 
enlarged  section  of  an  unfiltered  data  set  from  the  bottom  sonar  is  seen  in  Figure  3-3. 

The  analysis  of  the  noise  present  in  the  signals  led  to  the  development  of  a  digital  filter 
that  screens  out  points  more  than  1 5  units  away  from  the  current  average  value.  Points  that 
pass  this  screen  are  included  in  the  updated  moving  average,  consisting  of  the  ten  most 
recent  valid  points.  In  order  to  prevent  the  loss  of  possibly  valid  data  representing  a 
significant  change  in  a  feature’s  topography,  points  .screened  out  are  saved  in  a  b'lffer.  If  a 
sequence  of  ten  points  are  screened  out,  then  those  ten  points  are  used  to  develop  a  new 
average,  and  the  buffer  is  cleared.  When  dealing  with  the  forward  sonar,  the  screening  must 
allow  for  changes  in  the  range  signal  due  to  the  forward  motion  of  the  AUV  at  its  current 
speed.  Typically,  this  results  in  a  screen  of  25  -  30  units,  rather  than  the  20  used  for  the  side 
and  bottom  sonars.  The  data  set  seen  in  Figure  3-3  is  shown  in  Figure  3-4  after  filtering. 
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Figure  3-2.  Illustration  of  Sonar  Board  Averaging.  One  sonar  board  has  been 
switched  between  two  transducers  every  2  seconds  (20  pings).  Ranges  for  one 
head  are  shown  by  dots,  ranges  for  the  second  head  are  shown  with  asterisks. 
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Figure  3-3.  Unfiltered  Range  Data  Set.  Data  from  the  bottom  so¬ 
nar.  Range  is  in  interface  units,  where  1  unit  =  0.023  ft. 


Figure  3-4.  Filtered  Range  Data  Set.  Filtered  values  are  shown 
with  “x’s”,  original  data  (dots)  from  Figure  3-3  is  also  shown  for 
comparison.  Range  is  in  interface  units,  where  1  unit  =  0.023  ft. 

B.  SONAR  INSTALLATION 
1.  AUV  Nose  Mount 

The  four  sonar  transducers,  with  associated  cones,  currently  installed  in  the  AUV  II 
are  affixed  to  an  aluminum  mounting  bracket,  so  that  they  are  mutually  orthogonal.  There 
is  a  sonar  oriented  directly  forward,  directly  downward,  to  the  left,  and  to  the  right  of  the 
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Al'\’  body.  Since  the  fiberelasN  nose  is  acoustically  opaque  for  the  sonar  frequencie>  in  u>c. 
holes  were  cut  in  the  nose  to  expose  the  openings  of  the  shield  cones.  A  thin  plastic  sheet, 
acoustically  transparent,  was  placed  over  the  openings  to  maintain  the  hydrodynamic  prop¬ 
erties  of  the  nose.  The  watertight  cables  for  the  sonars  are  connected  to  a  watenight 
connection,  mounted  on  the  forward  bulkhead  of  the  AUV.  which  connects  to  the  sonar  pro¬ 
cessing  boards.  The  sonar  mount  is  shown  in  Figure  3-5. 


Figure  3-5.  AUV  II  Sonar  Installation.  Shield  has  been  removed  from  forward 
sonar  to  show  transducer.  Watertight  connection  is  at  upper  left. 
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2.  Computer  Interface 

Due  to  power  supply  limitations,  there  are  currently  two  sonar  processing  boards 
installed  in  the  AUV  II.  The  four  transducers  are  connected  to  the  two  boards  through  a  set 
of  four  microswitches,  so  that  one  board  serves  two  transducers  (Figure  3-6).  Each 
microswitch  is  software  controlled  to  enable  the  selection  of  a  pair  of  sonar  transducers  for 
use.  The  switch  interface  to  the  CPU  is  via  the  GESPIA-3A  parallel  interface  board. 
Additionally,  the  external  keying  signal  is  provided  to  each  sonar  board  via  the  GESPIA- 
3A.  To  effect  a  controlled  sonar  ping,  the  appropriate  pair  of  transducers  are  selected  for 
connection  to  the  sonar  boards,  and  then  the  boards  are  keyed  via  the  parallel  interface.  This 
simultaneous  keying  precludes  the  crosstalk  problems  discussed  earlier  in  this  chapter. 

The  analog  range  signal  (0-10  v  DC)  provided  by  the  sonar  processor  is  interfaced 
to  the  CPU  via  the  GESDAC-2B  analog-to-digital  converter  board.  This  interface  provides 
a  resolution  of  4096  units,  with  programmable  gain  control.  The  sonar  installation  uses 
unity  gain,  with  full  scale  representing  the  maximum  selected  range  (4096  units  =  10  volts 
=  30  meters). 
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Figure  3-6.  Sonar-Computer  Interface  Diagram. 


IV.  EXTRACTION  OF  LINEAR  FEATURES 
OF  OBSTACLES 

Regression  analysis  is  used  in  many  applications  to  find  linear  approximations  for  sets 
of  discrete  data  points.  In  the  case  of  underwater  obstacles  and  the  AUV  sonar,  data  points 
generated  by  sonar  returns  from  obstacles  may  be  used  to  generate  linear  features  of  the  ob¬ 
stacles  that  can  be  used  to  describe  the  obstacles  in  terms  of  the  environmental  model.  This 
linear  feature  extraction  enables  the  system  to  perform  pattern  matching  with  the  environ¬ 
mental  database  to  allow  navigational  position  updating,  or,  in  the  case  of  a  previously 
unknown  obstacle,  the  obstacle  can  be  added  to  the  environmental  database. 

This  section  discusses  the  application  of  the  least  squares  fit  method  to  sonar  data  and 
the  extraction  of  linear  features  of  obstacles.  These  linear  features  are  then  described  in 
terms  of  the  AUV’s  world  environmental  model  to  allow  pattern  matching  or  database  up¬ 
date.  [Ref.  23] 

A.  LEAST  SQUARES  FIT  METHOD 
1.  Coordinate  Transformation 

In  determining  global  coordinates  for  the  data  points,  we  use  the  AUV’s  dead 
reckoning  (DR)  position  (a:^,  y^)  and  heading  orientation,  \|/,  with  respect  to  the  global 

system,  and  the  sensor’s  orientation  with  respect  to  the  AUV  body  (Figure  4-1).  For  the  side 
and  forward  sonars,  the  effects  due  to  roll  and  pitch  are  negligible  and  can  be  ignored  due 
to  the  inherent  stability  of  the  AUV  in  these  axes.  Similarly,  the  bottom  sonar  range  is 
affected  only  by  the  pitch  angle,  with  effects  due  to  roll  being  minimal,  and  the  range  being 
independent  of  heading.  Note  that  the  heading  angle,  \\f,  is  measured  in  a  clockwise  fashion 
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Figure  4-1.  World  Coordinate  Conversion.  Diagram  illustrates  parameters  for 
a  range  return  from  the  left  sonar. 
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around  the  z  axis  by  a  gyroscope.  The  DR  position  is  determined  by 


x'  =  X  +  At  xux  cos  (V|/) 


(Eq4.1) 


y'^  =  y^  +  Atxux  sin  ( y) 


(Eq4.2) 


where  At  =  0.1  secs  and  u  is  an  of  the  velocity  along  the  vehicle’s  longitudinal 

velocity.  This  estimate  for  the  velocity  is  a  primary  source  of  DR  error.  Coordinates  for  the 


data  point  (p^^,  p^)  from  the  left  sonar  are  generated  as  follows: 


Px  =  x  +  range  X  cos  (y- 7i/2) 


(Eq4.3) 


p^  =  y.  +  range  x  sin  (\j/  -  n/2)  . 


(Eq4.4) 


2.  Linear  Regression  Principles 

We  first  discuss  how  to  extract  a  linear  feature  from  a  set  of  point  data  by  the  least 
squares  fit  method.  The  linear  feature  is  the  simplest  one  among  all  the  abstract  geometric 
features,  and  is  easily  obtained  by  range  sensors  from  an  orthogonal  world.  Suppose  a  set 
R  of  positions  for  an  envelope  of  an  object  in  a  plane  is  given  from  a  range  sensor. 


R  =  {(x.,  yj)|/=  1 . n)  . 


The  moments  mjj^  of  R  are  defined  as 


'ik  =  X  (0  <j,  k<  2,  and  j  +  k<2) 


Notice  that  =  n  .  The  centroid  C  of  is  given  by 


wio  '"oix.  X 

C  =  K — ,  — )  =  (p  >  pJ  . 

'"oo  '”oo  ^  y 


The  secondary  moments  around  the  centroid  are  given  by 


(Eq4.5) 


(Eq  4.6) 


(Eq4.7) 


(Eq4.8) 
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(Eq4.9) 


"ll=  X  = 

1  =  1 


m 
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m 


00 


'*  M* 

w  V  /■  \2  /  OK 

^02=  2/  =  "*02 ~  (Eq4.10) 

j  =  1  00 

We  adopt  the  parametric  representation  (r,  a)  of  a  line  with  constants  r  and  a.  If  a  point 
P=  U.  y)  satisfies  an  equation 

r  =  j:cosa  +  >sina  {—ii/l<a<n/2)  (Eq4  li) 

then  the  point  p  is  on  a  line  L  whose  normal  has  an  orientation  a  and  whose  distance  from 
the  origin  is  r  (Figure  4-2).  This  method  has  an  advantage  in  expressing  lines  that  are  per¬ 
pendicular  to  the  X  axis.  The  point-slope  method,  where  y  =  mx  +  b,  is  incapable  of 
representing  such  a  case  (am  =  <»,  6  is  undefined).  The  residual  of  point  p-  =  ix^  y-)  and 
the  line  L  =  (r,  a)  is  j:-cosa  +  >j  Sina-r  .  Therefore,  the  sum  of  the  squares  of  all  re¬ 
siduals  is 


n 

5  =  X  ('•-^,cosa-y,sina)^  (Eq4.l2) 

i  =  1 

The  line  which  best  fits  the  set  of  points  is  supposed  to  minimize  5.  Thus  the  optimum  line 
(r,a)  must  satisfy 


dr  9a 


(Eq4.I3) 
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Figure  4-2.  Representation  of  a  Line.  The  distance  from 
the  point  p  to  L  is  the  residual. 

Thus, 


(Eq4.14) 

=  2  (r/7ZQo-/rzjoCosa-mojSina) 

=  0 
and 

"‘lO  '"01  . 

r  =  — cosa+ — sina  =  u  cosa  +  u  sina  (Eq4.i5) 

'"oo  '"oo  ^  ^ 

where  r  may  be  negative.  Substituting  r  in  (Eq  4.12)  by  (Eq  4.15), 
n 

S  =  5^  ((x.-upcosa+  (y.-p^sina)^  .  (Eq4.16) 

i  =  1 


as 


^  =  2]^  (r-A:,cosa-y,sina) 


I  =  1 


1  =  1 


i=  1 


sina 


i  =  1 


J 
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Finally, 


^  =  2  ^  ( (jc.  -  n^)  cosa  +  (> .  -  ^I^)  sina)  (-  (x- -  4^)  sina  +  (>  ■  -  4^)  cosot  j 
i=  1 

n 

~  2  5^  ( (y^  -  ^  -  (jCj  - 11 J  sinacosa  + 

i=  1 
n 

2  X  ^yi  ~  (cos^«  -  sin^a) 

(Eq4.17) 

=  (Mq2- A/2o)  sin2a  + 2MjjCos2a 

=  0  . 

Therefore 

arctan2  (-2a/^  ^  Af^,,  -  A/20) 

a  =  - ^ _  (Eq4.18) 

The  solutions  for  the  line  parameters  generated  by  a  least  squares  fit  are  given  by  (Eq  4.15) 
and  (Eq  4.18). 

The  equivalent  ellipse  of  inertia  for  the  original  n  points  is  an  ellipse  which  has  the 
same  moments  around  the  center  of  gravity.  and  are  moments  about  the 

major  and  minor  axes  respectively  (Figure  4-3), 

^ major  -  (''^20  ^02)  ~  -^(^02"  ^20)  /4-(-A/jj 

^  minor  ~  ^ "I  ^  20^  /4  +  A/jj 


(Eq4.19) 

(Eq4.20) 
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Figure  4-3.  The  Equivalent  Ellipse  of  Inertia.  De¬ 
termined  for  the  set  of  points  R,  represented  by  the 
line  L. 


The  diameters  on  the  major  axis  and  on  the  minor  axis  of  the  equivalent  el¬ 
lipse  are 

^minor  ~  ^J^majo/'^OO  (Eq4.2]) 

^major  “  .  (Eq4.22) 

We  define  p,  the  ellipse  thinness  ratio.,  to  be  the  ratio  of  ^nd  d^ajor. 


P  = 


^ minor 


major 


(Eq  4.23) 


A  small  p  means  a  thin  ellipse;  as  p  increases  toward  1,  the  ellipse  degrades  to  a  circle  rep¬ 
resenting  a  thick  line  or  “blob”  of  points.  We  will  use  p  as  an  additional  measure  of  the 
linearity  of  a  set  of  points. 

The  residual  of  a  point  p,  =  (jt,  y,)  is 


6.=  cosa-t- sina  .  (Eq4.24) 
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Therefore,  the  projection,  p'-,  of  the  point  p-  onto  the  major  axis  is 

P'i  -  Cx,  +  5;Cosa,  y,  +  8.sina)  .  (Eq4.25) 

We  will  use  p\  and  p\  as  estimates  of  the  endpoints  of  the  line  segment  L  obtained  from 
the  point  set  R  by  the  least  squares  algorithm.  In  a  sequential  fitting  process  such  as  that 
employed  on  the  AUV,  only  the  first  point,  p,,  need  be  stored  during  processing.  Not  only 
is  the  equation  of  the  line  important,  but  the  estimation  of  both  endpoints  is  also  valuable 
information  for  sensor  based  navigation. 

B.  BEGINNING  A  NEW  LINE  SEGMENT 

While  the  least  squares  fit  method  provides  an  appropriate  estimate  for  a  line  passing 
through  a  set  of  points,  it  does  not  provide  a  method  of  determining  when  to  terminate  one 
line  segment  and  when  to  begin  another;  which  is  the  key  to  this  method.  This  decision  is 
critical  for  determining  the  best  linearity  fit  for  a  set  of  range  data.  For  example,  if  we 
receive  a  set  of  points  representing  two  perpendicular  surfaces,  the  least  squares  fit  method 
will  generate  a  single  line  that  accommodates  all  of  the  points.  This  fails  to  provide  an 
accurate  depiction  of  the  surfaces’  inherent  linear  features.  We  consider  a  set  of  range  data 
points  representing  a  corner  of  the  swimming  pool.  This  set  of  data  with  an  unterminated 
line  segment  fit  to  it  is  seen  in  Figure  4-4.  A  line  segment  terminated  with  the  described 
method  is  seen  in  Figure  4-5. 

1.  Test  for  Residuals 

To  determine  when  to  end  a  line  segment  we  perform  two  tests  on  the  current  line. 
The  first  test  checks  the  goodness  of  the  linearity  fit  for  the  most  recent  point, 

1,  >’,  +  i)  •  If  the  point  satisfies 

6 .  ^  j  <  maxic  1  x  G,c2)  (Eq  4.26) 

where  cl  and  c2  are  positive  constants  (typically,  cl  =  3.0  and  c2  =  0.4  ft.)  and  the  standard 
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deviation,  a,  is 


=  Pm.no/ (Eq4.27) 

then  the  point  can  be  included  in  the  current  line  segment. 


Figure  4-4.  Unterminated  Line  Fit.  Sonar  returns  from  a  corner 
of  the  swimming  pool  with  an  unterminated  line  fit  to  the  points. 
Axis  units  in  ft. 


2.  Test  for  Ellipse  Thinness 

The  second  test  uses  the  ellipse  information  discussed  above;  if  the  thinness  ratio 
for  the  line  is  smaller  than  the  third  constant,  c3,  then  the  set  of  points  is  still  acceptably 
thin.  We  have  modified  the  method  of  testing  the  thinness  ratio  from  that  used  in  [Ref.  15] 
by  scaling  the  thinness  criteria,  c3,  based  on  the  length  of  the  line.  If  c3  is  allowed  to  remain 
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Figure  4*5.  Terminated  Line  Segment.  Least  squares  fit  line  seg¬ 
ments  terminated  by  applying  tests  described  in  text.  Here,  cl  - 
3.0,  c2  =  0.4,  c3  -  0.1  initially.  Axis  units  in  ft. 


constant,  then  the  ellipse  thinness  remains  constant;  however,  as  the  line  grows  longer,  this 
means  that  the  acceptable  width  of  the  ellipse  also  grows.  Using  another  actual  data  set,  we 
can  see  in  Figure  4-6  that  the  line  is  skewed  slightly  with  respect  to  the  predominant  linear 
feature.  This  is  due  to  the  points  representing  the  swimmer  “pulling”  the  line  down  because 
they  still  fell  within  the  thinness  ratio.  For  larger  scale  features,  this  means  that  a  change  in 
a  feature  will  not  be  recognized  until  it  has  become  large  enough  to  fall  outside  of  the  thin¬ 
ness  criteria.  As  an  example,  a  line  segment  that  is  100  feet  long,  developed  using  an  ellipse 
thinness  ratio  of  0.1,  will  have  a  possible  width  of  10  feet.  This  potential  width  determines 
the  necessary  minimum  size  for  another  feature  to  force  a  termination  of  the  line  segment 
based  on  a  constant  thinness  ratio. 
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To  preclude  this  problem,  we  scale  the  thinness  ratio  as  a  function  of  length.  The 
staning  thinness  ratio  is  determined  as  the  desired  accuracy  of  the  smallest  feature  in  the 
environment,  with  two  feet  being  our  chosen  minimum  usable  size.  Due  to  the  physical 
dimensions  of  the  swimming  pool,  the  maximum  length  is  set  at  120  feet.  At  the  small  end 
of  the  spectrum,  we  have  fixed  the  thinness  ratio  at  0.1,  or  2.4  inches  for  the  smallest 
features,  while  at  the  other  extreme,  a  thickness  of  1.2  feet  is  acceptable,  which  requires  a 
ratio  of  0.01.  At  intermediate  lengths,  the  thinness  ratio  is  determined  by 


p  =  0.1-  (0.09- 


{MQ.Q- length) 
1311.11 


) 


0.1015 


length 

1311.11 


(Eq  4.28) 


With  this  adaptive  ratio,  the  feature  in  Figure  4-7  is  depicted  in  a  much  more  realistic  fash¬ 
ion,  with  the  line  segments  falling  close  to  the  actual  walls,  and  the  swimmer  being 
outlined. 

If  any  point  fails  either  of  the  two  tests  described,  it  is  placed  into  a  buffer  which  is 
used  to  initiate  the  next  line  segment.  This  method  reduces  the  deleterious  effects  of  noise, 
while  maintaining  the  history  of  a  possible  feature  change  worth  noting.  When  a  line 
segment  is  temtinated,  the  length  is  checked  so  that  only  those  segments  longer  than  a 
specified  minimum  (typically  2  ft.)  will  be  processed  by  the  pattern  matcher. 


C.  DESCRIPTION  OF  FEATURES  IN  TERMS  OF  AUV  WORLD  MODEL 


1.  The  Environmental  Model 

Before  obstacle  recognition  and  positional  updating  can  take  place,  a  suitable 
environmental  model  must  be  defined.  This  environment  model  must  facilitate  the  three 
functions  of  path  planning,  positional  identification,  and  model  updating.  All  three  of  these 
functions  require  the  expression  of  the  environment  in  some  type  of  numerical  form. 
Therefore,  linear  features  are  defined  by  a  Cartesian  coordinate  system  where  some 
predefined  point  serves  as  the  origin  (see  Figure  4-1  for  a  depletion  of  the  pool’s  eoordinate 
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system).  In  this  manner,  all  features  may  be  expressed  in  terms  of  their  x-,  y-,  and  z- 
coordinates  where  the  linking  of  three  or  more  vertices  defines  a  polyhedron. 


Figure  4-6.  Constant  Ellipse  Thinness  Ratio.  A  constant  value 
(c3)  allows  the  actual  width  of  the  ellipse  to  grow  as  the  line 
lengthens,  producing  a  poor  fit  to  the  linear  feature.  Here  cl  = 

3.0,  c2  -  0.3,  c3  =  0.1.  Axis  units  in  feet. 

The  structure  used  to  link  the  points  of  a  polyhedron  is  a  linked  list.  This  data 
structure  is  allocated  in  memory  containing  the  a-,  y-,  and  z-coordinates  of  each  point. 
Pointers  are  then  used  to  specify  which  vertices  are  connected  to  form  the  surfaces  of  a 
polyhedron.  Additionally,  the  line  parameters  {r,  aj  defining  each  surface  are  stored  in  the 
list,  with  r  being  the  positive  distance  from  the  origin  to  the  line,  and  a  being  measured  in 
a  clockwise  fashion  similar  to  the  AUV’s  heading  y. 


Figure  4*7.  Adaptive  Ellipse  Thinness  Ratio.  An  adaptive  ratio 
(Eq  4.28)  maintains  a  more  linear  fit  to  the  data  points  than  using 
a  constant  ratio  such  as  in  Figure  5,  cl  =  3.0  and  c2  =  0.3.  Axis 
units  in  ft. 


2.  Position  Identification  and  Updating 

By  using  the  parametric  representation  of  a  line  (r,  a),  the  process  of  matching  is  a 
simple  matter  of  comparing  the  generated  parameters  with  the  parameters  stored  in  the 
environmental  model.  The  match  criteria  for  r  and  a  are  designed  to  account  for  all  known 
errors  in  generating  the  line  segment.  There  are  three  primary  errors:  (1)  positional 
uncertainty  or  navigational  error,  (2)  inaccuracies  due  to  the  assumption  that  all  echoes  are 
along  the  sonar’s  centerline  (cosine  effect),  and  (3)  inherent  sonar  range  error.  We  currently 
use  an  error  of  3  ft.  in  r,  and  0.34  radians  (20'^)  in  a. 
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If  a  generated  line  segment  matches  a  known  model  feature  with  or  without  an  error, 
then  it  is  possible  to  identify  the  AUV’s  position  more  precisely  and  to  update  the  position 
based  on  the  actual  range  to  the  feature,  if  an  error  exists.  Utilizing  the  range  to  the  feature, 
the  known  geometry  of  the  sonar  beam,  and  the  angle  of  the  sonar  relative  to  the  feature, 
an  accurate  normal  distance  can  be  computed.  A  single  sonar  is  normally  capable  of 
updating  the  AUV’s  position  in  one  dimension  at  any  given  time.  In  the  case  of  the  left 
sonar,  with  the  recognized  feature  parallel  to  the  x-axis,  the  AUV’s  position  along  the 
global  y-axis  would  be  determined  as  follows, 

yAUV  =  •'feature  “  ^  sin  .  (Eq  4.29) 

If  the  sonar  is  in  contact  with  a  feature  that  is  parallel  to  the  y-axis,  then  the  AUV’s  along 
the  x-axis  would  be  determined  as  follows, 

^AUV  =  •'feature  "  ^OS  (Eq  4.30) 

3.  Model  Updating 

If  no  match  is  found  in  the  environmental  model,  then  we  assume  that  the  line 
segment  was  a  result  of  a  previously  unknown  feature,  or  obstacle.  The  parameters  for  this 
feature,  developed  as  a  planar  surface,  are  stored  as  a  new  feature  in  the  environmental 
model.  This  information  may  be  needed  for  the  path  planning  module,  particularly  if  the 
new  feature  represents  an  obstacle  to  the  currently  planned  path.  Any  new  features  that  are 
added  to  the  model  will  also  be  displayed  graphically  during  post-mission  data  analysis. 
The  line  segment  parameters  (r,  a)  are  stored  with  the  feature  for  future  pattern  matching; 
hence,  a  previously  unknown  obstacle  becomes  a  known  environmental  feature. 

The  information  generated  by  the  least  squares  method  is  in  two  dimensions  only, 
while  the  AUV  is  operating  in  three  dimensions.  For  the  forward  and  side  sonars,  the  two 
dimensions  used  are  the  global  x  and  y  coordinates,  while  for  the  bottom  sonar  the  z 
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information  is  treated  as  ‘y  ’  data  for  use  with  the  algorithm.  Features  seen  by  the  side  sonars 
are  in  the  vertical  plane,  while  those  seen  by  the  bottom  sonar  are  in  the  horizontal  plane. 

Figure  4-8  shows  the  acoustic  signal  pattern  of  each  transducer  as  a  spherical 
section.  A  valid  range  data  point  could  be  produced  by  any  reflecting  surface  patches  of  an 
object  on  this  spherical  surface.  We  make  the  assumption  that  the  range  return  was  from 
the  beam’s  centerline.  While  it  is  possible  to  correct  this  range  once  the  orientation  of  the 
feature  is  known,  we  do  not  want  to  preclude  the  possibility  that  a  previously  unknown 
obstacle  of  unknown  orientation  may  be  producing  the  return.  The  errors  induced  in  making 
this  assumption  will  be  absorbed  by  the  least  squares  algorithm  to  some  extent,  with  the 
remaining  error  being  accounted  for  in  the  matching  process. 

If  a  line  segment  is  not  successfully  matched  to  a  known  surface  in  the 
environmental  model,  then  we  must  process  the  segment  for  further  use.  Once  the 
endpoints  are  determined,  a  surface  with  length  equal  to  the  length  of  the  line  segment,  and 
width  proportional  to  the  range  from  the  AUV  to  the  endpoint  at  the  time  it  was  obtained 
(Figure  4-9),  is  generated.  The  width  is  determined  as 

d  =  rsin(5°)  =  rx  0.087.  (Eq4.3l) 

At  the  selected  maximum  range  of  30  meters  (98.25  ft.),  d  =  2.bm,  (7.86  ft.).  Choosing 
this  width  accounts  for  the  fact  that  the  return  could  have  come  from  any  point  on  the  sur¬ 
face  of  the  spherical  section  created  by  the  signal.  Thus,  a  3-D  surface  is  produced  from  2- 
D  information.  The  generated  polygon  is  stored  as  a  set  of  nodes  in  the  environmental  mod¬ 
el  for  used  in  future  path  planning  and  post-mission  data  visualization.  At  this  time,  we  do 
not  provide  for  the  retraction  or  resizing  of  surfaces  based  on  subsequently  obtained  infor¬ 
mation. 
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transducer 


Figure  4-8.  Cross-sectional  View  of  a  Sonar’s  Beam  Pattern.  The  dis¬ 
tance  d  and  the  error  e  are  proportional  to  the  range.  The  curvature 
of  the  spherical  surface  is  exaggerated  for  illustration  purposes. 


Figure  4-9.  Projection  of  Corner  Points  for  3-D  Surface.  Corner  points 
generated  from  the  line  L.  The  distance  d  is  determined  by  (Eq  4.31). 


V.  SOFTWARE  PROCESSES  FOR  REAL¬ 
TIME  OBSTACLE  AVOIDANCE 


The  sonar  system  installed  in  the  AU  V II  vehicle  plays  an  integral  role  in  the  execution 
of  planned  missions.  The  system  interfaces  with  several  other  processes,  as  illustrated  by 
the  current  working  version  of  the  dataflow  diagrams  for  the  AUV  software  (Figure  5- 1  and 
Figure  2-3).  The  sections  below  describe  the  various  sonar  software  processes  and  the 
interfaces  to  the  other  processes  seen  in  Figure  5-1.  The  interface  with  the  environmental 
database  was  discussed  in  Section  C  of  Chapter  IV.  The  actual  code  associated  with  the 
sonar  processes  is  found  in  Appendix  B. 

A.  SONAR  SOFTWARE  PROCESSES 

The  process  READ  SONAR  DATA  (5.1)  in  Figure  5-1  obtains  the  current  range  value 
for  the  appropriate  sonar  by  reading  the  proper  channel  of  the  GESPAC-2b  DA/AD 
converter.  This  raw  data  is  then  used  to  update  the  moving  average  in  the  FILTER  DATA 
process  (5.2),  as  described  in  Section  A.6  of  Chapter  III.  The  obstacle _alert  flag  will  also 
be  set  here,  as  described  in  the  next  section.  The  new  average  range  is  then  used  by  the 
UPDATE  LINE  SEGMENT  (5.3)  process  to  develop  a  line  segment.  If  the  point  is  valid 
for  the  current  line  segment,  then  the  line  parameters  are  updated  and  compared  to  the 
known  obstacles  by  the  process  MATCH  LINE  TO  MODEL  (5.4).  If  no  match  is  found, 
then  the  new  obstacle  flag  is  set. 

If  the  new  data  point  causes  a  termination  of  the  current  line  segment  based  on  the  tests 
described  in  Section  B  of  Chapter  IV,  and  the  new  obstacle  flag  is  set,  then  a  new  obstacle 
is  developed  and  added  to  the  environmental  database  as  described  in  Section  C.3  of 
Chapter  FV  by  the  ADD  NEW  OBSTACLE  process  (5.7).  If  the  line  segment  is  terminated, 
then  all  variables  are  cleared  and  a  new  line  will  start  with  the  next  incoming  range  data 
point.  Once  the  match  process  (5.4)  is  complete,  a  position  update  (see  Section  C.2  of 
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Figure  5-1.  Sonar  Process  Dataflow  Diagram. 


Chapter  IV)  can  be  performed  if  the  obstacle  was  found  in  the  database,  i.e.  the  new_obsta- 
cle  flag  is  not  set. 

B.  INTERFACE  WITH  CONTROL  GUIDANCE  SYSTEM 

There  are  two  occasions  when  information  obtained  by  the  sonar  might  dictate  motion 
control  for  the  AUV.  The  first  instance  is  the  most  critical,  it  is  simple  obstacle  avoidance. 
The  second  instance  arises  when  it  becomes  desirable  or  necessary  to  map  the  extent  of  a 
previously  unknown  obstacle;  this  case  is  discussed  in  the  next  section.  Sonar  range 
information  that  indicates  a  potential  collision  in  any  direction  will  be  recognized  by  the 
FILTER  DATA  process  (5.2)  in  Figure  5-1. 

A  minimum  safety  range  for  each  direction  from  the  AUV  (forward,  left,  right,  down) 
is  stored  as  part  of  the  sonar  range  data  structure.  As  the  latest  range  reading  is  filtered,  the 
minimum  safety  range  is  compared  to  the  average  actual  range  for  that  direction.  If  any  of 
the  actual  ranges  fall  below  the  minimum  safety  range,  the  obstacle  alert  flag  bit  is  set  for 
that  sonar/direction.  This  flag  is  passed  to  the  AVOID  OBSTACLES  process  (8)  in  Figure 
5-1. 

The  AVOID  OBSTACLES  process  determines  which  course  of  action  to  follow  if  any 
of  the  flag  bits  are  set.  A  heading  change,  a  depth  change,  a  full  stop,  or  any  combination  of 
these  changes  may  be  necessary.  The  various  options  are  shown  in  Table  5-1.  The  currently 
installed  software  generates  an  “emergency  posture”  that  forces  a  turn  or  depth  change  to 
avoid  areas  flagged  as  containing  threatening  obstacles.  While  operating  in  the  NFS  swim¬ 
ming  pool  environment,  all  turns  are  ninety  degrees.  Since  the  AUV’s  dynamics  will  carry 
it  closer  to  an  obstacle  during  the  turn,  a  logical  lockout  system  prevents  the  sonar  ranges 
from  generating  any  more  alerts  until  the  AUV  is  within  20°  of  the  new  heading.  If  there 
are  no  known  lateral  obstacles,  an  obstacle  ahead  of  the  AUV  could  be  avoided  by  a  turn 
to  the  left  or  to  the  right.  The  default  in  this  case  is  a  right  turn. 
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TABLE  5-1.  AUV  OBSTACLE  AVOIDANCE  MANUEVERS 


Obstacle  Alert  Flag 

(Fwd,  right,  left,  bottom) 

Turn 

Depth  Change 

OXXO 

— 

— 

OXXl 

— 

ascend 

1101 

left 

ascend 

1100 

left 

1011 

nght 

ascend 

1010 

right 

— 

lllX 

stop 

(ascend) 

0  =  no  obstacle 

1  =  obstacle 

X  =  0  or  1 

C.  MOTION  ALGORITHM  FOR  OBSTACLE  MAPPING 

The  most  reliable  method  of  mapping  an  underwater  feature  is  to  control  the  motion 
of  the  vehicle  so  that  the  range  to  the  feature  remains  within  the  optimum  range  of  the 
sensor,  but  does  not  present  a  collision  danger.  As  described  in  Chapter  IV,  the  linear 
feature  extraction  method  provides  the  best  resolution  at  short  ranges;  therefore,  the 
minimum  safety  range  becomes  the  determining  factor  in  choosing  a  mapping  distance. 

Mapping  of  the  bottom  topography  with  the  down-looking  sonar  is  much  simpler  than 
maintaining  a  fixed  distance  laterally  from  a  feature.  As  noted  earlier,  the  heading  of  the 
AUV  has  no  effect  on  the  feature  extraction  of  targets  below  the  AUV.  Due  to  the  AUV’s 
inherent  stability,  pitch  and  roll  will  affect  the  bottom  sensor  only  in  extreme  cases.  A 
simple  terrain  following  altitude  controller  has  been  implemented  and  tested  in  the  NPS 
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swimming  pool.  The  control  law  used  was 

=  f^range  “  ’^ange)  +  k^^^eta^  +  k^q  (Eq  5.1) 

where  dels  is  the  commanded  dive  plane  angle,  range^^^  is  the  desired  altitude  above  the 
bottom,  range  is  the  actual  sonar  range  to  the  bottom,  0  is  the  pitch  angle,  and  q  is  the  pitch 
rate. 

Mapping  of  lateral  features  with  the  left  or  right  sonars  is  complicated  by  the  need  to 
control  heading  while  attempting  to  maintain  mapping  range  to  the  feature.  For  this  reason, 
a  simpler  scheme  involves  generating  intermediate  waypoints  and  allowing  the  guidance 
and  control  processes  to  maneuver  the  AUV.  Care  must  be  taken  to  ensure  that  a  large 
lateral  step  is  broken  down  into  smaller  steps  to  ensure  that  the  sonar  in  contact  can 
maintain  contact  during  the  maneuver.  This  means  heading  changes  of  no  more  than  five 
degrees  relative  to  the  surface  of  the  feature  to  obtain  the  highest  resolution.  The  desired 
feature  resolution  and  mission  task  will  ultimately  dictate  the  magnitude  of  these  turns. 

D.  INTERFACE  WITH  ONBOARD  MISSION  REPLANNER 

At  any  time  that  an  obstacle  is  detected,  either  as  a  collision  danger,  or  as  a  previously 
unknown  feature,  the  new_obstacle  flag  is  set.  This  flag,  in  conjunction  with  the 
obstacle  jilert  flag,  is  used  to  signal  the  mission  executor.  One  of  the  roles  of  the  mission 
executor  is  to  determine  from  these  flags  if  path  replanning  is  necessary,  or  if  obstacle 
mapping  may  also  be  required.  In  order  for  the  mission  replanner  to  accurately  assess  the 
path  to  the  goal,  it  must  have  a  complete  environmental  database.  If  an  emergency  turn  was 
made  due  to  an  obstacle  ahead  of  the  AUV,  it  may  be  necessary  to  generate  obstacle 
mapping  waypoints  to  determine  the  extent  of  the  obstacle.  This  area  of  research  is 
addressed  by  Wilkinson  [Ref.  24],  and  is  a  matter  of  ongoing  research. 
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VI.  EXPERIMENTAL  RESULTS 


A.  EXPERIMENTAL  RESULTS  OF  MODEL  MATCHING 

Satisfactory  tests  of  the  model  matching  algorithm  discussed  in  Chapter  IV  were 
conducted  with  the  AUV  operating  in  the  swimming  pool.  The  range  data  points  shown  in 
Figure  6-1  are  from  one  such  test.  Again,  the  errors  previously  discussed  are  manifested  in 
an  imperfect  match  to  the  pool  walls.  However,  by  accounting  for  these  known  errors  in  the 
matching,  matches  between  generated  line  segments  and  the  environmental  model  are 
possible. 


Figure  6*1.  Sample  Range  Data  Set.  Range  data  set  used  for  by  the  model 
matching  algorithm,  taken  during  a  test  in  the  NPS  swimming  pool.  Bold 
rectangle  represents  pool  walls. 
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Using  an  error  of  3  ft.  in  r,  and  0.34  radians  (20  degrees)  in  a,  the  generated  line 
segments  in  Figure  6-2  represented  as  dashed  lines  matched  features  in  the  model  correctly, 
with  no  false  matches  generate!. 


Figure  6-2.  Model  Matching  Results.  Line  segments  fit  to  the  data  set 
shown  in  Figure  6-1,  with  those  segments  that  were  identified  by  the  model 
matching  algorithm  shown  with  dashed  lines.  Numbers  represent  element 
of  model  matched,  with  “s9s”  representing  no  match.  Bold  rectangle  repre¬ 
sents  pool  walls. 


B.  REAL-TIME  OBSTACLE  AVOIDANCE 

The  software  processes  used  to  perform  obstacle  avoidance  were  discussed  in  Chapter 
V.  The  results  of  a  pool  test  mission  are  seen  in  Figure  6-3.  The  original  preplanned 
waypoints  for  the  mission  are  represented  by  an  “x”,  the  point  corresponding  to  the 
minimum  safety  range  is  marked  w'ith  an  “O”.  and  the  obstacle  avoidance  waypoint 
generated  by  the  AVOID  OBSTACLES  process  is  shown  with  a  “W”.  As  demonstrated  by 
this  test,  the  sonar  processes  and  gross  motion  control  discussed  in  Chapter  V  work  as 
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Figure  6-3.  Obstacle  Avoidance  Test.  AUV  track  is  shown  in  upper  box,  forward  sO' 
nar  range  is  graphed  below.  Obstacle  within  28  ft.  was  detected  at  points  marked 
with  “O”,  resulting  in  new  waypoints  (“W”)  to  replace  original  track  (“x”). 


Figure  6-4.  Altitude  Controller  Performance.  AUV  depth  was  commanded  to  be  3 
ft.  and  was  controlled  using  the  bottom  sonar.  Pool  depth  profile  during  mission  is 
shown  in  lower  graph. 
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VII.  SIMULATION  AND  POST-MISSION  3-D  DATA  MAPPING 

A.  AUV  MISSION  SIMULATOR 

1.  Overview 

The  role  of  simulation  in  a  project  such  as  the  AUV  n  project  is  very  beneficial, 
both  for  testing  new  processes  and  for  analyzing  actual  data  collected  during  a  test  mission. 
Some  of  the  benefits  of  the  current  simulator  include: 

•  Allows  software  developers  to  test  and  debug  new  modules  without  the  effort 
of  a  full  field  ♦est 

•  Provides  visual  display  of  sonar  returns 

•  Provides  the  ability  to  develop  and  observe  AUV  missions  before  they  are 
actually  run 

•  Provides  the  ability  to  visualize  the  results  of  a  mission  from  the  actual  data 
collected  during  a  test. 

The  AUV  mission  simulator  is  based  on  the  simulator  developed  by  Jurewicz  and 
detailed  in  [Ref.  25].  This  simulator,  running  on  a  Silicon  Graphics  IRIS  workstation, 
provides  a  3-D  graphical  presentation  of  the  NPS  AUV  II  as  it  moves  through  the  NPS 
swimming  pool,  or  the  Monterey  Bay.  The  simulated  AUV  is  controlled  via  either  mouse 
input  through  the  interface  screen,  or  by  manipulating  the  6-degree-of-freedom  Spaceball 
input  device.  Velocities,  accelerations,  and  position  changes  are  calculated  using  the 
control  inputs  for  motor  speeds  and  fin  deflections.  A  detailed  hydrodynamic  model  of  the 
AUV  is  used  to  compute  the  simulated  motion  of  the  vehicle. 

2.  Sonar  Simulation 

In  order  to  fully  simulate  the  AUV  in  the  pool  environment,  a  simulation  of  the 
sonar  signals  was  developed  using  graphics  ray  tracing  techniques.  Each  sonar  transducer 
is  modeled  as  a  point  source  for  a  sound  ray,  with  a  single  ray  emanating  from  the  source 
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in  the  direction  of  orientation  of  the  sonar.  Since  the  sonars  actually  emit  acoustic  energy 
in  a  cone  pattern,  six  additional  rays  are  simulated  to  form  the  outer  surfaces  of  the  cone, 
spreading  out  from  the  source  at  a  five  degree  angle  (see  Figure  7-1). 


Rays.  Seventh  ray  is  in  center,  along  sonar  axis. 

Each  ray  is  traced  in  a  recursive  fashion  as  it  intersects  the  polygons  forming  the 
pool  boundaries.  Each  point  of  intersection  is  stored  in  an  array,  and  the  angle  of  reflection 
is  calculated.  Each  point  then  becomes  a  new  source,  and  the  next  level  of  reflection  points 
is  calculated.  This  recursion  continues  until  a  level  of  four  reflections  is  reached.  If  the  total 
distance  travelled  in  reaching  a  point  exceeds  two  times  the  maximum  range  of  the  sonar 
(30  m),  then  that  ray  is  terminated  regardless  of  the  level  of  recursion,  since  it  would  arrive 
back  at  the  sonar  after  the  next  ping  started.  The  ray-tracing  code  is  adapted  from  an 
algorithm  found  in  Glassner  [Ref.  26]. 
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After  all  of  the  points  of  intersection  are  determined  and  stored,  the  point  found  to 
be  closest  to  the  sonar,  with  a  reflecting  angle  within  the  sonar’s  field  of  view,  is 
determined.  For  a  point  to  be  closest,  the  distance  that  the  ray  has  travelled  to  that  point 
added  to  the  Euclidean  distance  from  the  point  to  the  sonar,  must  be  smaller  than  for  any 
other  point.  This  distance  is  then  divided  by  two,  and  a  point  is  projected  from  the  sonar  in 
the  direction  of  its  orientation,  since  we  assume  that  all  sonar  returns  come  from  points 
along  the  sonar  beam’s  axis.  Any  points  that  were  reached  by  one  or  more  reflections  will 
appear  farther  from  the  sonar  than  the  actual  object  (see  Figure  7-2).  This  becomes  most 
pronounced  when  the  sonar  is  directed  toward  the  vertex  of  a  concave  corner. 


Figure  7-2.  False  Apparent  Range  Caused  by  Re¬ 
flection. 


B.  POST-MISSION  REPLAY 

All  of  the  actual  test  missions  run  in  the  NPS  pool  provide  a  mission  data  file  that  is 
downloaded  from  the  AUV  at  the  end  of  each  test  run.  This  information  is  utilized  for 
parameter  analysis  and  sonar  performance  evaluation.  The  replay  capability  of  the  original 
AUV  simulator  has  also  been  expanded  to  accept  the  data  files  from  actual  missions.  The 
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data  file  is  loaded  onto  the  graphics  workstation  where  mission  replay  is  selected.  Each 
control  parameter  is  used  to  position  the  control  surfaces,  and  the  positional  information  is 
used  to  detemiine  the  AUV’s  position  and  orientation  for  display. 

Additionally,  the  range  values  for  each  sonar  are  projected  from  the  appropriate  sonar 
location  for  display.  As  the  AUV  moves  through  the  mission  replay,  the  track  is  displayed, 
as  well  as  the  actual  sonar  returns.  This  capability  allows  the  users  to  determine  if  the  pre¬ 
mission  simulation  was  accurate,  or  if  the  AUV  may  have  reacted  to  a  situation  not  foreseen 
by  the  testers,  as  evidenced  by  an  unexpected  sonar  return  or  track  change.  The  completed 
replay  of  an  actual  AUV  pool  mission  is  seen  in  Figure  7-3,  with  the  actual  recorded  track 
and  sonar  information  labelled. 

The  3-D  surfaces  discussed  in  Chapter  IV  representing  unknown  obstacles  can  be 
displayed  as  well.  This  capability  would  have  much  more  significance  for  a  vehicle 
operating  in  open  waters,  or  in  an  area  where  the  environmental  database  was  sparse.  This 
capability,  although  limited  by  the  resolution  and  number  of  sensors,  may  well  support 
many  major  missions  for  AUVs. 
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Figure  7-3.  Graphical  Mission  Replay.  View  of  simulated  pool  from  above  follow¬ 
ing  replay  of  mission  sonar  and  track  data.  Apparent  errors  are  due  to  DR  naviga¬ 
tion  inaccuracies. 
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VIII.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

This  work  addressed  four  primary  objectives,  as  reiterated  here: 

•  How  can  data  from  ultrasonic  sensors  be  best  utilized  to  recognize  obstacles 
during  operation  of  the  NPS  AUV  II? 

•  What  is  the  optimal  configuration  for  ultrasonic  sensors  on  the  AUV  n  to 
provide  obstacle  detection  and  terrain  data  collection  for  post-mission 
analysis? 

•  What  type  of  motion  algorithm  will  best  provide  collision  avoidance  and 
obstacle  feature  extraction? 

•  How  can  sensor  data  be  utilized  in  post-mission  analysis  to  generate  a  3-D 
terrain  model? 

After  conducting  the  development,  testing,  and  evaluation  of  the  various  processes 
described  in  this  work,  we  have  reached  the  following  conclusions  in  response  to  the 
questions  posed  in  the  objectives: 

•  The  linear  feature  extraction  method  presented  here,  having  been  initially 
developed  on  a  land  vehicle  in  two  dimensions  [Ref.  15],  proved  itself  robust 
enough  to  provide  useful  information  for  an  AUV  operating  in  three 
dimensions,  as  well. 

•  Given  the  current  availability  of  four  sonar  systems,  the  orthogonal  mounting 
arrangement  discussed  in  Chapter  n  proved  satisfactory  for  obstacle 
avoidance,  altitude  control,  and  feature  extraction. 

•  In  the  swimming  pool  environment,  the  processes  discussed  for  obstacle 
avoidance  and  terrain  data  collection  were  demonstrated  to  be  satisfactory. 

•  The  extension  of  an  existing  graphical  simulation  to  include  a  mission  replay 
capability  provided  a  satisfactory  tool  for  analysis  of  3-D  mission  data. 

A  key  factor  in  the  successful  development  of  this  work  was  the  availability  of  a 
readily  adaptable  graphical  simulator.  The  effort  involved  in  conducting  pool  tests  with  the 
AUV,  and  competing  research  requirements,  precludes  frequent  testing  of  individual 
aspects  of  the  vehicle’s  systems.  Access  to  a  dynamic  simulator  ensured  that  basic 
algorithm  development  and  testing  could  be  done  independent  of  the  actual  vehicle. 
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The  goal  of  classifying  obstacles  is  currently  being  pursued  by  others  involved  in.the- 
NPS  AUV  research  project.  Due  to  the  inherent  noise  in  the  sensors  and  the  low  resolution 
of  the  sonars  as  currently  installed,  classification  of  objects  must  be  limited  to  those  objects 
in  excess  of  two  feet  in  size.  The  calculation  of  position  based  on  inertial  sensing  is 
currently  being  developed  and  should  provide  more  reliable  results  in  the  future,  as  well. 

B.  RECOMMENDATIONS 

The  capabilities  of  NPS  AUV  II  continue  to  grow  as  new  processes  and  systems  are 
developed  and  installed.  In  order  to  enhance  the  obstacle  avoidance  capabilities  of  the 
vehicle,  the  following  specific  recommendations  are  provided  for  future  work; 

•  Provide  for  the  installation  of  more  sonar  transducers  to  provide  increased 
coverage  of  vehicle  surroundings. 

•  Eliminate  the  on-board  averaging  performed  by  the  current  sonar  processing 
boards. 

•  Install  an  accurate  inertial  system  to  provide  more  accurate  positioning  data. 

•  Consider  providing  additional  processing  capabilities  to  ensure  that  the 
required  control  system  sampling  rates  are  maintained. 

•  Develop  simulation  capabilities  ahead  of  any  new  functionality  that  is  installed 
in  the  vehicle. 

The  NPS  AUV  II  project  should  continue  to  be  a  viable  research  effort  for  many  years, 
and  these  recommendations  "’■e  made  to  ensure  that  future  efforts  will  be  able  to  build  on 
this  and  other  current  work. 
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APPENDIX  A 


AUV  DATA  DICTIONARY 


Baseline  System  (vl.l) 

Symbols  for  Describing  Data 
NAME  =  Description 

Description  +  Description...  (denotes  aggregate  structure  “+”) 

m/n  {Description}  max  (denotes  multiple  instances  if  min  not  specified,  assume 

empty;  if  max  not  specified,  assume  infinite) 

<Description  I  Description>  (denotes  mandatory  “<>”  choice  “I”) 

(Description)  (denotes  optional  item  “()”) 

Predefined  Data  Names 

CHAR  =  Letters,  numbers,  space,  punctuation 

STRING  =0(  CHAR)  239 

EMPTY  =  absence  of  value 

DIGIT  =  0..9 

DAY=  1  {DIGIT)! 

MONTH  =1  {DIGIT}  2 

YEAR=  4{DIGIT}4 

DATE  =  DAY  +  MONTH  +  YEAR 

INTEGER  =1  {DIGIT} 

REAL  =  {DIGIT}  +  .  +  {DIGIT} 

SECOND=2{DIGIT}2 

MINUTE  =  2  {DIGIT}  2 

HOUR=  2  {DIGIT}  2 

TIME  =  HOUR  +  MINUTE  +  SECOND 
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NOTE:  BOLD  TYPE  NAMES  REFER  TO  current  Baseline  System  DFD 


Point_3D  = 

X  = 

REAL  + 

y  = 

REAL+ 

z  = 

REAL 

L,inear_velocity_3D= 

u  = 

REAL  + 

V  = 

REAL+ 

w  = 

REAL 

Linear_accel_ 

.3D= 

u_dot= 

REAL  + 

v_dot= 

REAL  + 

II 

o 

REAL 

Attitude_3D= 

phi  = 

REAL  + 

theta= 

REAL  + 

psi  = 

REAL 

Rotational_velocity_3D= 

P  = 

REAL  + 

q  = 

REAL  + 

r  = 

REAL 

Rotational_acceL3D= 

p_dot= 

REAL  + 

q_dot= 

REAL  + 

r_dot= 

REAL 

Fin_angle  = 

REAL 

Polygon  =  3{Point_3D} 

Posture  = 

Position  =  Point_3D  + 

Velocity  =  Linear_velocity_3D  + 

Acceleration=  Linear_accel_3D  + 

Attitude  =  Attitude_3D  + 
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Rotation_vel=  Rotational_velocity_3D  + 
Rotation_acc=Rotational_accel_3D 
Time  =  TIME 

Mission_requirement= 

Start_point  =  Point_3D  +  TIME  + 

Intennediate_point={Point_3D  +  TIME}  + 
Goal  =  Point_3D  +  TIME 

Mission_type  =  INTEGER 

Path  =  2  {Posture) 

Referenceposture  =Posture 

Current_posture=  Posture 

Replan_request=  BOOLEAN 

Range_data= 

Range=  REAL  + 

Error=  REAL 

Sonar_data=  Range_data  + 
Transducer_no=INTEGER 

Track_data=  Current_posture  +4{  Sonar_data  }4 

Commanded_posture=Posture  + 

Mode  =  INTEGER 

Emergency_posture=Posture 

Status  =  BOOLEAN 

System_status=n  { BOOLEAN }  n 
Note:  n  is  number  of  systems  reporting 

Obstacle_alerl-BOOLEAN 

Know'n_obstacles=  (Polygon) 

Ncw_obstacle  =  1  { Polygon )  I 
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Position_update  =  Point3  +  TIME  + 

Confidence_factor  =INTEGER 

Inertial_data  =  RotarionaLvelociry  3D  +  Anitiide_3D  + 

Depth  =  REAL  + 

Fwd_speed=  REAL  + 

Mag_hdg=  REAL 

Control_signal  = 

Fin_deflection=8{Fin_angle)8  + 
Rt_main_rpm={DIGIT)3  + 

Lt_main_rpm={DIGIT}3  + 

Fwd_hor_rpm={DIGIT)3  + 

Aft_hor_rpm=  {DIGIT}  3  + 

Fwd_ver_rpm={DIGIT}3  + 

Aft_ver_rpm=  {DIGIT}  3 

Control_posUions  =  Control_signal 
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APPENDIX  B 


AUV  CODE 


AUV.H 

Main  header  file  for  the  auv  control  code. 

************************************************************************************* I 

#include  <sidio.h> 

#ifndef  EXTERN 
#define  EXTERN  extern 
tidcfinc  INIT(x) 

#endif 

#ifndef  MAIN 
#definc  EXT  extern 
#endif 

#ifdcf  MAIN 
#dcfinc  EXT 
#undef  MAIN 
#endif 


#definc  DAC2B_ADDR  OxFFF00040  /*  GESDAC-2b  addr  */ 

#dcfinc  DAC_LSB_OFFSETOx2 
#dcfine  DACLADDR  OxFFFOOOOO 

^define  ADC1_ADDR(DAC1  ADDR +  0x11) 

Udefine  ADC1_MSB  0x0 
^define  ADC1_LSB  0x2 
#define  ADC1_CMD_REG  0x4 
#define  ADC1_STATUS_REG  0x4 
#dcfinc  ADC  1_BUSY  0x40 

#definc  ADC2_ADDR  0xFFF00020 
#definc  ADC2_CH_GAIN  0x0 
#define  ADC2_STATUS_REG  0x2 
^define  ADC2_DATA  0x1 
#define  ADC2_CMD_REG  0x2 

#define  Mn_BASE  0xFFF00700 

#dcfinc  V1A0_ADDR  0xFFF00080 
^define  ORB_IRB  0 
#define  ORA_IRA  1 
#definc  DDRB  2 
#define  DDRA  3 

#dcfinc  T1C_L  4 
#define  T1C_H  5 


/*  GESPIA-3A  bus  addr  */ 
r  GESP1A-3A  i/o  reg  B  */ 
/*  GESP1A-3A  i/o  reg  A  */ 
/*  Data  direction  reg  */ 

/*  Data  direction  reg  */ 
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#define  T1L_L  6 
#defineTlL_H7 
#definc  T2C_L  8 
#define  T2C_H  9 
^define  ACR  1 1 
#dcfine  PCR  12 
#de6ne IFR 13 

#define  FRONT_RUD_TOP  0 
#define  FRONT_RUD_BOT  1 
#define  FRONT_PL_RIGHT  2 
^define  FRONT_PL_LEFT  3 
#define  REAR_RUD_TC  >P  4 
#define  REAR_RUD_BOT  5 
#define  REAR_PL_RIGHT  6 
#define  REAR_PL_LEFT  7 


#define  RIGHT.MOTOR  0 
#define  LEFT_MOTOR  2 
#definc  SUPPLY  1 

#dcfine  RIGHT_MOTOR_RPM  0 
#define  LEFT_MOTOR_RPM  1 
#define  ROLL_ANGLE_CH  7 
^define  PITCH, ANGLE_CH  6 
#define  ROLL_RATE_CH  9 
#define  PITCH_RATE_CH  8 
#define  YAW_RATE_CH  10 


I*  port  register  A  /  data  direction  A  */ 
I*  control  register  A  */ 


I*  control  register  B  */ 


struct  MFI_PIA  { 

unsigned  short  pra; 

unsigned  short  era; 

unsigned  short  prb; 

unsigned  short  erb; 

}; 


EXT  unsigned  char  Read_PortA(),Read_PoriB(); 
EXT  unsigned  short  Read_PortAB(); 


EXT  FILE  *outfpl; 

EXT  FILE  *outfp2; 

EXT  ini  dala_lcngih; 

EXT  int  loopcni; 

EXT  int  end_test; 

EXT  int  wrap_couni; 

EXT  doublet; 

EXT  double  rptn; 

EXT  double  main_motor_delta  1  ,main_motor_delta2; 
EXT  int  main_motor_voltl ; 

EXT  int  main_motor_volt2; 

Function  declarations 


EXT  void  uscr_intcrfacc(),  control_modulc0; 
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EXT  void  loop_dataO.  zero_dataO,  controLsurfaccO,  rccord_daia_on(); 

EXT  void  record_data_offO,  rccord_daia(): 

EXT  void  inilialize_dacsO,inilializc_adcs(); 

EXT  void  rudderO.pIanesO; 

EXT  void  main_motors_offO,aIivcO; 

EXT  voiddacl0.dac2b(); 

EXTintadcl0^c2(); 

EXT  double  headingO,pitch_angle0.roll_angleO,calc_psiO; 

EXT  double  roll_raie_gyroO,yaw_raie_gyro(),pitch_rate_gyro(); 

EXT  double  depthO: 

EXT  double  righl_m_rpni(),lcfl_ni_rpm(); 

EXT  double  get_speedO; 

EXT  unsigned  short  psi_bit_old; 

EXT  double  old_angIe; 

EXT  double  dg_offsei  jc_psi,k_r; 

EXT  double  k_z,k_iheta,k_q; 

EXT  double  k_specd; 

EXT  double  ki_spccd; 

EXT  double  timejimit; 

EXT  int  num_brcak; 

EXT  unsigned  siart_dwcll; 

EXT  int  (Erection; 

EXT  double  x_com(20],y_com[20],z_com[201; 

EXT  double  psi_com_offsci: 

EXT  double  psi_com_sst: 

EXT  double  psi_com; 

EXT  double  dclta_x_y_/', 

EXT  double  x,  x_init; 

EXT  double  y,  y_inil;  /*  inii  pos  input  in  user_interface()  */ 

EXT  double  sp^; 

EXT  int  roll  rate  0,pitch_raic_0,yaw_ratc_0; 

EXT  int  rolfO.pirch.O; 

EXT  int  z_valO; 

EXT  int  pointer; 

EXT  int  speed_array[  11]; 

EXT  double  speed_limit,  delta_speed; 

EXT  double  della_sum_specd; 

EXT  int  tick,  tickl,  tick2,  currjick,  mask,  count; 

EXT  double  value; 

EXT  long  date.lime; 

EXT  short  day; 

EXT  char  ans; 

I  ^i^iitiJtim*****^*^*****^^**^******************^^************ 

Sonar  related  data  items/functions/structurcs 

#define  NEWTYPE(x)  (x  *)(malioc((unsigned)sizcof(x))) 

^»*«4t4i**4>**«*4t*4i**3t>»4i**4i4i**««*:4(*3*(%****«**)|<*:^**«**  )*>«*** 

Sonar  switch  addresses  for  use 
with  GESPIA  interface 

^define  SONAR_SWl  OxOE 
#define  SONAR_SW2  0x00 
#dcfine  SONAR_SW3  OxOB 
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#define  S0NAR_SW4  0x07 
#define  SONAR.TRIGl  0x10 
#definc  SONAR_TRIG2  0x20 

!  ^L*^*ttmi>*m*******L*’ir*********************************** 

Constant  values  for  use  with 
sonar  line  fitting  and  obstacle 
avoidance 

#define  CONVERT_TO_FEET  0.02398 
#define  MIN_OBSTACLE_RNG  25.0 
#define  AVG.PTS  10 
#define  MAX_RNG_DIFF  15.0 
#define  MAX_RNG_DIFF_FWD  25.0 
#define  MAX_PTS  1200 
#define  DEG_TO_RAD  0.017453 
#define  FALSE  0 
#define  TRUE  1 

/*  Constants  for  line  seg  package  */ 

#definecl  3.0 
^define  c2  .6 
#define  c3  .20 
#define  MIN_NO_PTS  3 
#define  MAX_BAD_PTS  3 

#ifndef  SONAR 
#define  EXT  extern 
#endif 

#ifdef  SONAR 
#define  EXT 
#undef  SONAR 
#endif 

Variable  declarations 

•  ********** ******«*«»****»*****  j 

EXT  int  error,  range,  sw_cnt,  sw_com,  bad_mg,  bad_updates,  rangc_index; 

EXT  int  b_range,  bad_pLno,  obstacle_alcrt,  new_obstacle; 

EXT  double  range  1; 

EXT  double  range2; 

EXT  double  range3; 

EXT  double  rangc4; 

EXT  double  error  1; 

EXT  double  error2; 

EXT  double  avg_mg,  range_com,  k_rangc,  max_dclta_r,  max_dclta_thcta; 

EXT  double  range  arrayflO]; 

EXT  double  bad_pl_buffcr[M  AX_B  AD_PTS)[2]: 

EXT  double  poolI6][2J; 

EXT  double  max_delta_r,  max_dclta_theta;  /*  input  in  user  _interface()  */ 

!  *«***4>«4r***********«4r******i^«**i»**4^*****«i»4>***4t»*4<*)t>4< 

Function  declarations 

EXT  void  get_init_avg0,  get_avg_mg(),  initialize_sonars(); 

EXT  void  ping_sonars(),  sonars_on0,  set_up_sonars(); 

EXT  double  atan20;  /*  not  provided  by  math.h  in  current  compiler  */ 

EXT  void  init_pooI0; 

EXT  void  match_modc!(): 

EXT  void  resctJincO; 


/*  GESDAC2  units  ->ft  */ 

/*  Obs  avoidance  mg  (ft)  */ 

/*  filter  avg  window  size  */ 

I*  filter  bandpass  sides/bottom*/ 
/*  filler  bandpass-fwd  sonar  */ 


/*  sigma  factor  */ 

/*  deviation  const  */ 

/*  ellipse  thinness  ratio  */ 

/*  least  sqrs  min  */ 

/*  least  sqrs  bad  pt  buffer  size*/ 
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EXT  void  recoid_line_on(); 
EXT  void  recordJine_olT(): 
EXT  void  line_segmcnt_iniiO; 
EXT  void  compute  Jine_seg(); 
EXT  void  end_line_scgmcntO: 
EXT  void  swap_inil_poinis(); 
EXT  void  convert_coords(); 

Data  structures 


lypedef  struct 

{ /*  Data  for  each  sonar’s  line  segment  *! 

double  sonar_data_pts[MAX_PTS][2];  /*  ijng  */ 

double  planc_pis[4][3]; 

double  sgmxx, 

sgmyy, 

sgmxy, 

sgmyx, 

sgmx, 

sgmy, 
r_sonar, 
theta_sonar; 
double  mux, 
muy, 

muxx, 
muyy, 

muxy, 
delia_x, 
delta_y, 
sgmx2, 
sgmy2; 

double  sgm_deita_sq. 

sigma, 

d_major, 

d_minor, 

m_major, 

m_minor; 

double  delta_line, 

siart_pt_x, 

start_pt_y, 

end_pt_x, 

cnd_pt_y; 

double  line_length; 

int  i_s, 

j-S. 

n_s, 

k_s, 

end_pi_no, 

siart_pt_no, 

bad_pt, 

too_long, 

too_far_apart, 

course_change; 

nt  depih_changc, 

range_pt, 

loc, 

min_x, 

max_x; 

)  LINE_SEGMENT,  »L1NE_SEG; 
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lypedef  struct 

1 

int  rangejndex, 
bad_mg, 
bad_updaics, 
switch, 
trigger. 
sonar_id; 

doublerange_array  [  AVG_PTS  ] , 
range_toial, 
avg_mg. 
niax_mg_diff, 
inin_obs_mg, 
raw_mg: 

}RANGE_DATA: 

AUVMAIN.C 

Main  control  code  for  the  au\  . 

#define  MAIN 

#include  <ermo.h> 

#include  <setsys.h> 

#include  <math.h> 

^include  “auv.h” 

unsigned  char*dacl_a  =  DAC1_ADDR; 
unsigned  char*dac2b_a  =  DAC2B_ADDR; 
unsigned  char*adcl_a  =  APCLADDR; 
unsigned  shon*adc2_a  =  AE)C2_ADDR; 
unsigned  shoa*viaO  =  VIA0_ADDR; 


void  init_vars(); 
void  init_clockO; 
void  check_clock(); 

main() 

{ 

R  ANGE_D  ATA  *  fronl_sonar,*  lefl_sonar, 
*right_sonar,*bouom_sonar; 
LINE_SEGMENT  *lcfl_line; 

count  =  0; 
mask  =  OxOOOOffff; 
obstacle_alert  =  0x0; 
new_obsuicle  =  0; 

fronl.sonar  =  NEWTYPE(RANGE_DATA); 
left.sonar  =  NEWTYPE(RANGE_DATA); 
right_sonar  =  NEWTYPE(RANGE_DATA); 
bottom_sonar  =  NEWTYPE(RANGE_DATA); 

ieftjine  =  NEWTYPE(LINE_SEGMENT); 


/*  4  Channels  of  DAC  ADA-1  DAC  */ 
/*  8  channels  of  DAC  DAC-2B  •/ 

/*  Ibchannelsof  ADC  ADA-1  */ 

/*  16  channels  of  ADC  ADC-2  */ 

I*  PIA  addr  */ 


init_vars(); 
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Ioop_data0;  /*  Get  z_coni  &  x_com  &  y_com  arrays  */ 

userJnterfaccQ; 

initiaiize_dacsO: 

initialize_adcs(); 

printfC'  Position  AUV  for  Directional  Gyro  Offset  MeasurementXn”); 

printfC*  Rate  Gyro  zero  measurementNn”); 

prinlfC*  Hit  Any  Key  When  ReadyNn”); 

ans  =  getcharO; 

ans  =  getcharO; 

set_up_sonars(front_sonar,  righl_sonar,  lcfl_sonar 

bouom_sonar);  /*pass  in  all  four  sonars  in  this  order*/ 

sonars_on(front_sonar,  left_sonar); 

f*  pass  in  two  sonars,  must  be  one  from  each  board->  (front  or 
right)  followed  by  (left  or  bottom)  */ 

zero_dataO; 

prinlf(“Stariing\n”); 

psi_bit_old  =  Read_PortAB(MFI_BASE); 
psi_bit_old  &=  0x3FFF: 

alive(10,start_dwell);  /*  Wag  fin  every  10  seconds  for  total 

duration  of  siarl_dwcll  seconds  */ 


record_data_on(); 

record_line_on(); 

initialize_sonars(front_sonar,  left_sonar); 

initializejine(lefljinc,  3); 

while  (end  test)  /*  lOHz  control  loop  */ 

( 

init_clock(); 

get_avg_mg(front_sonar); 

get_avg_mg(lcft_sonar); 

update_Iine_seg(left_line); 

conirol_module(front_sonar,  right_sonar,  left_sonar,  bottom_sonar); 
ping_sonars(froni_sonar,  left_sonar); 
chcck_clock();  /*  timed  loop  control  */ 


main_motors_off(); 

record_data_off(); 

endjine_segment(left_line); 

record_line_on'(): 


}/*  end  mainO  */ 
init_vars() 

***********«*******»**********:tr4i*******«*4,4.*»******:|,  ! 

void  init_vars() 
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I 

data.lengih  =  0; 
loopcnl  =  0; 
end_test=  1; 
wrap_count  =  0; 

1  =  0.0; 

inain_moior_volll  =  512; 
inain_motor_volt2  =  512; 
direction  =  1; 
old.angle  =  0.0; 
psi_com_offsci  =  0.0; 

X  =  0.0; 

y  =  0.0; 
sp^  =  0.0; 
pointer  =  0; 

delta_sum_speed  =  0.0; 
)!*  end  init_globals0  V 

inil_clock() 

void  init_clock0 

{ 


_sysdat^3,&lime,&daie,&day,&tick); 
lickl=(tick  &  mask); 

)/*  end  inii_clock()  */ 

check.clockO 

void  check_cloclc() 

{ 

_sysdate(3,&time,&date,&day,&tick): 
tick2=(  tick  &  mask ); 
if  (lick2  <  tick  I) 
tick2  =  tick2+100; 
va]ue=abs(tick2-tick  1 ); 
while  (value  !=  10) 

{ 

if  (tick2<  tickl) 
tick2=tick2-»-100; 
value=abs(tick2-tick  1 ); 
_sysdalc(3,&limc,&daic,&day.&lick); 
tick2=(  tick  &  mask); 

) 

)/*  end  chcck_clock()  •/ 


jtr*m*******m*m***m************************^*********’¥**********^***’h****  ************** 

The  following  sonar  code  is  found  in  the  file  asonar.c. 

AUV  sonar  modules  called  from  main  loop  of  control  program  or 
from  other  sonar  modules. 

#define  SONAR 

#includc  “auv.h” 
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set_up_sonarsO 

;^^^-^^iti****'tt************Tilf****************************  I 

void  set_»ip_sonars(sonarl,  sonar2,  sonar3,  sonar4) 

RANGE_DATA  *sonarl,  *sonar2,  *sonar3,  *sonar4  ; 

I 

sonar  l->bad_mg  =  0;  /*  from  */ 
sonar2->bad_mg  =  0;  /*  right  */ 
sonar3->bad_mg  =  0;  /*  left  */ 
sonar4->bad_mg  =  0;  /*  bottom  */ 

sonar  l->bad_updates  =  0; 
sonar2->bad_updates  =  0; 
sonar3->bad_updates  =  0; 
sonar4->bad_updatcs  =  0; 

sonarl->switch  =  S0NAR_SW1; 
sonar2->switch  =  SONAR_SW2; 
sonar3->swiich  =  SONAR_SW3; 
sonar4->swiich  =  SONAR_S\V4; 

sonarl->triggcr=  SONAR_TRIGl: 
sonar2->triggcr  =  SONAR_TRIG  1 ; 
sonar3->trigger  =  SONAR_TRIG2; 
sonar4->trigger  =  SONAR_TRIG2; 

sonar  l->range_toial  =  0; 
sonar2->range_total  =  0; 
sonar3->rangc_toial  =  0; 
sonar4->rangc_toial  =  0; 

sonar  l->max_mg_diff  =  MAX_RNG_DIFF_FWD; 
sonar2->max_mg_diff  =  MAX_RNG_D1FF; 
sonar3->max_mg_din'=  MAX_RNG_DIFF; 
sonar4->max_mg_diff  =  MAX_RNG_D1FF; 

sonar  l->min_obs_mg  =  28.0; 
sonar2->min_obs_mg  =  8.0; 
sonar3->min_obs_mg  =  8.0; 
sonar4->min_obs_mg  =  3.0; 

)/*  end  sel_up_sonars()  */ 

sonars_on0 

void  sonars_on(sonarl,  sonar?) 

RANGE_DATA  ♦sonarl,  *sonar2; 

I 

viaOfDDRB]  =  0ji3F;  /*  set  data  direction  regs  */ 

via0[DDRA]  =  0x00; 

viaO[ORB_IRB]  =  sonarl->switch  &  sonar2->switch;  /*lum  on  sonar  */ 

tsieepfl);  /*  switch  debounce  time  10ms  V 

)/*  end  sonars_on()  */ 

!  4i^*4ti^*4ci^*«**4'*»****«**4r«******«**»4t**4>***4i«<«*« ***««'** 

iniiialize.sonarsO 

»*******»*****«i«*******************************4>****  j 

void  inilialize_sonars(sonarl,  sonar2) 

R ANGE_DATA  *sonarl .  *sonar2; 


/*  values  for  example  only  */ 

/*  values  are  mission  dependent  */ 
I*  units  in  feel  */ 
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int  i; 

for  ( i  =  0;  i  <  20;  ++i) 

{ 

viaO[ORB_IRBl  =  (sonarl->switch  &  sonar2->switch’)  I 

sonar  l->lrigger  I  sonar2->iriggcr;  /"iriggcr*/ 

viaO[ORB_IRB]  =  sonarl->switch  &  sonar2->switch;  /*  clear  */ 

tsleep(5);  /*  wait  for  max  range  return  50  ms*/ 

1 


get_init_avg(sonarl,  sonar2); 

)/*end  initiaIize_sonars()  */ 

iniUalize_lineO 

m****m***’i>*m******************************i**********  j 

void  initialize_line(line,  loc) 

LINE_SEGMENT  •line; 
int loc; 

( 

line->loc  =  loc;  /*sct  location  id  #  */ 

rcsct_Iinc(linc);  /•  inil  vars  to  zero  */ 

) 


j  «**•***•*********»4^********I|^  ****<•<»»*****«»*»*»*  ****** 

get_init_avg() 

****************************************************  j 

void  getJnit_avg(sonarl,  sonar2) 

RANGE  DATA  •sonarl,  *sonar2; 

{ 

int  i; 

for(i  =  0;  i  <  AVG.PTS;  ++i) 

( 

ping_sonars(sonarl,  sonar2); 
tslecp(5); 

sonarl ->range_array[i]  =  (float)adc2(2,0);  /*  board  #1  */ 
sonar2->rangc_array[iJ  =  (float)adc2(3,0);  /*  board  #2  ♦/ 
sonar  l->rangc_total  +=  sonarl ->range_array[i]; 
sonar2->range_total  +=  sonar2->range_array[i]; 


sonarl ->avg_mg  =  sonarl->range_total/AVG_PTS; 
sonar2->avg_mg  =  sonar2->rangc_iolaI/AV'G_PTS; 
sonarl ->range_index  =  0; 
sonar2->range_indcx  =  0; 

)/*  end  get_init_avg()  */ 


! ***************************************************** 
ping_sonars0 

**************************************************** ! 

void  ping_sonars(sonarl ,  sonar2) 

RANGE_DATA  *sonarl,  *sonar2; 

{ 


viaO(ORB_lRB]  =  (sonarl ->switch  &  sonar2->switch)  I  sonarl ->triggcr  I 
sonar2->trigger; 

viaO[ORB_IRB|  =  sonarl ->swiich  &  sonar2->swiich;  /*  dear  */ 

)/*  end  ping_sonars()  ♦/ 
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j  «****«**»**** 

gei_avg_mg() 

«*«»** **»>)< ******»»**»**** *  j 

void  get_avg_rug(sonar) 

RANGE_DATA  *sonar; 

( 

inti; 


if((sonar->switch  ==  S0NAR_SW1)  II  (sonar->switch  ==  SONAR_SW2)) 

I 

sonar->raw_mg  =  adc2(2,0); 

) 

else 


{ 

sonar->raw_mg  =  adc2(3,0); 

) 


filier_range_daia(sonar); 

)/*  end  gci_avg_mgO  */ 

fillcr_range_daia() 

void  filter_range_data(sonar) 

RANGE_DATA  *  sonar; 

{ 

if  ((sonar->raw_mg  >  sonar->avg_mg )  11 

(fabs(sonar->raw_mg  -  sonar->avg_mg’)  <=  sonar->max_mg_difO  H 
(sonar->bad  mg  >=  MAX_BAD_PTS)) 

{ 

sonar->range_lotal  =  sonar->range_total  - 

sonar->range_array[sonar->rangeJndex]; 
sonar->range_aiTayfsonar->rangeJndcxI  =  sonar->raw_mg; 
sonar->range_lotal  +=  sonar->raw_mg; 
sonar->avg_mg  =  sonar->rangc_loml/AVG_PTS; 
sonar->rangc_indcx  =  (sonar->rangc_indcx  +  1)  %  AVG_PTS; 

if(sonar->bad_mg  >=  MAX  BAD_PTS) 

{ 

++sonar->bad_updatcs; 

) 

if(sonar->bad_updaies  >=  MIN_NO_PTS) 

( 

sonar->bad_mg  =  0; 

1 

) 

else 

{ 

++sonar->bad_mg; 

) 

if((sonar->avg_mg  *  CONVERT_TO_FEET)  <=  sonar->min_obs_mg) 

switch  sonar->sonar_num 

( 

case  1: 

( 

obstacle_aIcrt  =  obsiacIe_alcrt  1 0x8; 
break; 

} 
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case  2: 

{ 

obslaclc_alcri  =  obsiaclc_alcri  1 0x4; 
break; 

] 


case  3: 

{ 

obstacle_a]ert  =  obstacle_alert  1 0x2; 
break; 

) 


case  4; 

( 

obsiacle_alert  =  obstacle_alcri  10x1; 
break; 

) 

) 

) 

)/*  end  filier_range_daia()  *! 

update_line_seg() 

void  update_line_seg(line) 

LINE_SEGMENT ’line; 

{ 

if(line->range_pt  <=  MIN  NO_PTS) 

{ 

line_scgmcni  iniulinc); 

1 

if(line->range_pt  >  MIN_NO_PTS) 

( 

Une_seg_compulc(linc); 
if(line->bad_pt  >=  MAX_BAD_PTS) 

{ 

end_l  ine_scgmeni(l  inc); 
resetjine(line); 
swap  inil_poinls(linc); 

) 

) 

) 

resci_line() 

***«******4>********************************'<>*****»**^ 

void  resei_line(line) 

LINE.SEGMENT  ’line; 

( 

line->n_s  =  0; 
line->Ls  =  0; 
line->start_pt_no  =  0; 
line->end_pt_no  =  0; 
linc->rangc_pt  =  0; 
line->sgmx  =  0.0; 
line->sgmy  =  0.0; 
line->sgnix2  =  0.0; 
line->sgniy2  =  0.0; 
line->sgmxy  =  0.0; 
line->sgm_dclia_sq  =  0.0; 
line->max_x  =  0; 
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Une->min_x  =  0; 

) 

line_segincnl_initO 

Initialize  a  line  segment  and  its  associated  variablcs/llags 
Called  f jom  sonar_rangeO  in  sonar_bay.c. 

•  «»*****«»»»***»****»**4i**»»**** ****»»•»**••*«»*»»*»  ! 

void  line_segment_tnit(line) 

LINE_SEGMENT  *line; 

{ 

/*  Read  in  first  points  to  establish  initial  line  segment  */ 
line->line_length  =  0.0; 
line->bad_pt  =  FALSE; 

I*  accumulate  variables  */ 
line->sgmx  +=  linc->sonar_data_pts[linc->n_sll01; 
line->sgmy  +=  line->sonar_data_pts[line->n_s]ilJ; 
line->sgmx2  +=  SQR(line->sonar_data_pts[line->n_s][0]); 

Iine->sgmy2  +=  SQR(line->sonar_dala_pts(linc->n_s]|  1)); 
linc->sgmxy  +=  line->sonar_data_pLslline->n_sllO]  * 
line->sonar_data_pts[Iine'>n_sl[l  ]; 
linc->cnd_pt_no  =  linc->n_s; 

if(linc->sonar_data_pts[line->n_sJ[Oj  <  Iinc->sonar_daia_pisllinc->min_xJlOJj 
line->min_x  =  line->n_s; 

if(line->sonar_data_pts[line->n_s](01  >  line->sonar_data_ptsfline->max_x][01) 
line->max_x  =  linc->n_s; 

/*  Update  the  counters  */ 

if  (linc->rangc_pt  ==  1) 

linc->siarl_pi_no  =  linc->n_s; 

++line->n_s; 

++line->i_s;  /*  current  line  segment  point  counter  */ 

if  (line->range_pt  >=  M1N_N0_PTS)/*  use  x  data  points  for  first  segment  */ 

/•  Calculate  first  line  segment  values  */ 
linc->mux  =  linc->sgmx  /  linc->i_s; 
line->muy  =  linc->sgmy  /  linc->i_s; 

line->muxx  =  iinc->sgmx2  -  (line->sgmx  *  line->sgmx)  /  line->i_s; 
line->muyy  =  linc->sgmy2  -  (linc->sgmy  *  linc->sgmy)  /  line->i_s; 
line->muxy  =  line->sgmxy  -  (iinc->sgmx  *  iinc->sgmy)  /  linc->i_s; 

line->end_pt_no  =  line->n_s  -  1; 

line->theta_sonar  =  (atan2((-2.0*line'>muxy),(line->muyy-line->muxx)))  /  2.0; 

line->r_sonar  =  linc->mux  *  cos(liiic->thcta_sonar)  +  line->muy  ’  sin(line->iJieui_sonar); 
for  (linc->j_s  =  0;  line->j_s  <  MlN_NO_PTS;  ++linc->j  s) 

[ 

line->k_s  =  (line->j_s  +  line->siart_pt_no); 

linc->sgm_dclia_sq  +=  SQR(linc->sonar_data_pts[linc->k_s][0]  -  linc->mux) 

*  SQRcos(linc->thcla_sonar)); 

line->sgm_delta_sq  +=  SQR(line->sonar_data_ptsllinc->k_sj[l)  -  linc->muy) 

*  SQR(sin(line->lheia_sonar)); 

line->sgm_delta_sq  +=  2.0  *  (Iine->sonar_daia_pts[line->k_s][0]  -  linc->mux) 

*  (linc->sonar_data_pts[linc->k_s][li  -  linc->niuy) 

*  cos(linc->thcta_sonar)  *  sin(line->thcia_sonar); 

) 


linc_scg_compulc0 
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Read  in  subsequent  data  points,  after  a  line  segment  has  been 
initialized  and  more  range  values  arc  obtained 

line_seg_compute(line) 

LINE_SEGMENT  *linc: 

I 

[*  Calculate  test  values  */ 

line->sigma  =  line->sgm_delta_sq  /  line->i_s: 

r*  Test  new  point  for  linearity  fit  */ 

line->dclta_line  =  Iine->sonar_data_pts[linc->n_s][0]  *  cos(line->thcta_sonar) 

+  line->sonar_data_pts(line->n_s][l]  •  sin(line->lhcia_sonar) 

-  line->r_sonar; 

if  (((fabs(line->delta_line))  <=  (line->sigma  *  cl))  II  ((fabs(line->dclta_line))  <=  c2)) 

( 

line->sgmx  +=  linc->sonar_data_pts[linc->n_s](01; 
line->sgmy  +=  line->sonar_data_ptsiline->n_sjil]; 
line->sgmx2  +=  SOR(line->sonar_data_pts[linc->n_s][0]); 

Iinc->sgmy2  +=  SQR(linc->sonar_daia_pts(linc->n_s]|ll); 

line->sgmxy  +=  linc->sonar_data_ptsllinc->n_s][0)  *  linc->sonar_daia_pis[linc->n_sJll]; 

line->mux  =  line->sgmx  /  (line->i_s  +  1); 

linc->muy  =  Iinc->sgmy  /  (linc->i_s  +  I); 

linc->muxx  =  Iine->sgmx2  -  SQR(line->sgmx)  /  (line->i_s  +  1 ); 

line->muyy  =  Iine->sgmy2  -  SQR(line->sgmy)  /  (line->Ls  +  1); 

line->muxy  =  line  >sgmxy  -  (line->sgmx  *  line->sgmy)  /  (linc->i_s  +  1); 

/*  calculate  ellipse  values  */ 

line->m_major  =  (line->muxx  +  linc->muyy)  /  2.0  -  sqrt((line->muyy  -  linc->muxx) 

*  (]inc->muyy  -  line->muxx) /4.0  +  SQR(linc->muxy)); 
linc->m_minor  =  0inc->muxx  +  linc->muyy)  /  2.0  +  sqn((linc->muyy  -  linc->muxx) 

*  (iinc->muyy  -  linc->muxx)  /4.0  +  SQR(linc->muxy)); 
line->d_major  =  4.0  •  sqrt(fabs(linc->m_minor  /  (line->i_s  +  1))); 
line->d_minor  =  4.0  *  sqrt(fabs(linc->m_major  /  (line->i_s  +  1))); 

/*  Test  new  point  for  ellipse  line->thinness  •/ 
if  ((line->d_minor  /  line->d_major)  <  c3) 

( 

line->cnd_pt_no  =  linc->n_s;  /•  update  end  point  */ 
linc->bad_pt  =  0;  /* reset  moving  bad_pi  counter  */ 

/* 

*  update  line  segment  parameters  to  include  new 

*  point 

*/ 

line->thcia_sonar  =  (atan2((-2.0  *  linc->muxy), 

(linc->muyy  -  line->muxx)))  /  2.0; 
line->r_sonar  =  line->mux  •  cos(!ine->theta_sonar)  +  linc->muy 

*  sin(line->thcta_sonar); 

line->sgm_dclta_sq  +=  2.0  *  (Iinc->sonar_data_pts[linc->n_s][0]  -  line->mux) 

*  (line->sonar_data_pts[linc->n_s][ll  -  Iine->muy) 

*  cos(linc->thcta_sonar)  *  sin(linc->lhcta_sonar); 
line->sgm_dclta_sq  +=  SQR(line->sonar_data_pts[line->n_s][l]  -  line->muy) 

*  SQR(sin(line->theta_sonar)); 

line->sgm_delta_sq  +=  SQR(line->sonar_data_plsnine->n_s][0]  -  line->mux) 

*  SQR(cos(linc->lheta_sonar)); 

if(line->sonar_data_pts[line->n_s](0]  <  line->sonar_data_pts[linc->min_x][0]) 
line->min_x  =  line->n_s; 

if(linc->sonar_data_ptslline->n_s]lOj  >  linc->sonar_data_pts|line->max_x][0]) 
linc->max_x  =  line->n_s; 

++linc->n_s; 

++line->i_s; 
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line->(ielia_x  =  Iine->sonar_data_pis[line->start_pt_no][0] 

-  Iine->sonar_(laia_pts[line->cnd_pt_no][0]; 
linc->dclia_y  =  linc->sonar_daia_pu[linc->siari_pl_no][l] 

-  line->sonar_data_pts[line->end_pt_no][l]; 
linc->Iinc_lcneth  =  sqri/'SOR(^linc->dclia_x)  +  SORninc->delia_v')): 

1 

else 

( 

bad_pt_buffer[line->bad_pl](01  =  Iine->sonar_data_pts[line->n_s][0]; 
bad_pt_buffer[linc->bad_pti[l]  =  line->sonar_data_ptsiline->n_s][l]; 
++linc->bad_pt; 

) 

) 

else 

{ 

bad_pt_buffer[line->bad_pi][0)  =  linc->sonar_dataj3is[linc->n_s][0]; 
bad_pl_buffer[line->bad_pt]il]  =  line->sonar_daiaj)tsiline->n_s][l]; 
++line->bad_pt; 

) 


y  »♦»*♦**♦♦*♦*♦*****»*»*  **3»!*5k***»>:»3|C*:*Jtr2).*******:f*****»* 

end_line_segmeniO 

Wrap  up  a  line  segment  if  bad  data  pt,  course  change,  depth  change, 
or  segment  max  length  reached. 

******************************  ******************  j 

void  endjine_segment(linc) 

LINE_SEGMENT  *line; 

{ 

int  i; 

double  linc_anglc; 

/•  start  new  line  segment  */ 

linc->sgmx  =  0.0; 
line->sgmy  =  0.0; 
linc->sgmxy  =  0.0; 
line->sgmx2  =  0.0; 

Iinc->sgmy2  =  0.0; 
line->sigma  =  0.0; 
linc->sgm_dclta_sq  =  0.0; 

/♦  close  out  old  segment,  convert  radius  to  positive  value  first  */ 
if  (linc->r_sonar  <  0) 

{ 

linc->thcta_sonar  =  180  *  DEG_TO_RAD  +  linc->thcia_sonar; 
linc->r_sonar  =  -1  *  linc->r_sonar; 

) 

/*  determine  start  and  end  points  on  the  computed  line  segment  */ 
linc->start_pt_x  =  Iinc->sonar_daia_pts[linc->start_pl_no)[0]; 
line->start_pt_y  =  line->sonar_data_pts(line->start_pt_no][l]; 

Iine->end_pt_x  =  Iine->sonar_data_ptsfline->end_pt_no][01; 
line->cnd_pt_y  =  linc->sonar_daia_pts(line->end_pi_no)l  1  J; 
line->delta_line  =  line->start_pt_x  *  cos(linc->ihcta_sonar) 

+  linc->start_pt_y  *  sin(linc->theta_sonar)  -  fabs(line->r_sonarj; 
line->start_pl_x  =  iine->stari_pt_x  -  (line-:  dclta_line  *  cos(Iinc->theia_sonar)); 
line->.start_pt_y  =  linc->.start_pi_y  -  (linc->dc!ta_linc  *  sin(line->thcta_sonar)); 
linc->dclta_linc  =  linc->cnd_pi_x  *  cos(linc->llicia_sonarj  +  linc->cnd_pi_y  ’  siiHlmc->llK'Ui_M)naj ) 
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-  fabs(line->r_sonar); 

Iine->end_pt_x  =  line->end_pt_x  -  (Une->dclia_line  *  cos(line->theia_sonar)); 
line->end_pi_y  =  Une->end_pi_y  -  (line->delia_line  *  sin(line->lheia_sonar)); 
Une->dclta_x  =  line->start_pi_x  -  line->end_pt_x; 
line->dclia_y  =  linc->sian_pl_y  -  linc->cnd_pi_>  ; 
line->line_length  =  sqrt(SQR(line->delta_x)  +  SQR(line->delia_y) 

+  SQR(Iine->sonar_data_pts[line->staii_pt_no]  [2] 

-  line->sonar_data_pts[line->end_pl_no]l2])); 

if((line->linejength  >=  24.0)  &&  (line->loc  !=  1)) 

( 

match_model(line); 

if(pool_surfacc  ==  9)  /*  no  match  */ 

{ 

creaie_ne  w_obstacle(l  ine); 
recordjine(line); 

new_obstacle  =  1 ;  /♦set  flag  */ 

) 

) 

++linc->n_5;: 


crcaie_new_obstaclc() 

void  create_new_obstaclc(iine) 

LlNE_SEGMENT*linc; 

( 

if  (linc->loc  ==  4)/’'  Bottom  sonar  */ 

{ 

line_angle  =  atan2(linc->sonar_data_ptslline->start_pt_no][2] 

-  linc->sonar_data_pts[linc->cnd_pt_nolf2], 

Iinc->sonar_daia_pis[linc->siart_pt_no][0]  -  linc->sonar_daia_pts|linc->cnd_pi_no 
line_anglc  =  linc_anglc  -  PIOVER2; 

1  else 


line_angle  =  atan2(line->sonar_data_ptslline->stan_pt_no][l] 

-  line->sonar_daia_pts[Iinc->end_pt_no][  1], 
sqrt(SQR(line->sonar_data_pts[line->start_pt_no][0] 

-  Iinc->sonar_daia_pts[linc->cnd_pt_no]|0]) 

+  SQR(line->sonar_data_pts[linc->siart_pt_no](2] 

- 1  inc->sonar_daUi_pts[  line->end_pt_no]  [  2]))); 

) 

if  ((Iinc->loc  ==  4)  /*  Bottom  sonar  */ 

( 

offsetlc  =  line->sonar_data_pLs[line->start_pt_no]l3)  *  0.08715.“)  *  cos(linc_anglc); 
ofTsetls  *  line->sonar_data_pts[Iine->start_pt_no][3)  *  0.087155  *  sin(line_anglc); 
offsct2c  =  linc->sonar_data_pLs[linc->cnd_pt_nol[3]  *  0.087155  *  cos(line_anglc); 
offsct2s  =  linc->sonar_data_pls[linc->end_pt_no]l3J  *  0.087155  *  sin(Iinc_anglc); 

)  else 


offsetlc  =  linc->sonar_data_pLs(Iinc->start_pt_no][3]  •  0.087155  *  cos(linc_anglc); 
offsctls  =  linc->sonar_dala_pLs[Iinc->.start_pt_no][31  *  0.087155  *  sin(linc_anglc); 
offsetlc  =  lmc->sonar_data_ptsllinc->cnd_pt_no][31  ’  0.087155  ’  cos(,liiic_ungU'j; 
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offsei2s  =  linc->sonar_data_pls[Une->end_pi_no][3]  *  0.087155  *  sin(line_angle); 

) 

sei_plane_pir(line); 
line->n_s  =  0; 
line->siarijpi_no  =  0; 
line->end_pl_no  =  0; 

for  (i  =  0;  i  <  100;  ++i) 

Hne->sonar_data_pis[i][3]  =  0.0; 


) 

swap_inii_points() 

^im**************************************************  I 

void  swap_init_points(line) 

LINE_SEGMENT*line; 

{ 

int  i; 

line->Ls  =  0; 

line->range_pi  =  0; 

for(i  =  0;  i  <  line->bad_pt;  ++i) 

( 

line->sonar_daia_pts[line->n_sJ[0]  =  bad_pt_bulTer[i][0]; 
line->sonar_data_pts[line->n_s][l]  =  bad_pi_buffcr[i][l]; 
++line->rangc_pt; 
line  scgment_inU(linc): 

) 

) 


converi_coords() 

void  conven_coords(line) 

LINE  SEGMENT  *line; 

I 

linc->sonar_daia_pls[line->n_s][0]  =  x  +  avg  mg  *  cos(psi  -  1.57079) 

•  CONVERT_TO_FEET; 

line->sonar_daui_pis[line->n_s][l]  =  y  +  avg  mg  *  sin(p.si  -  1.57079) 

*  CONVERT_TO_FEET; 

++linc->rangc_pt; 


sel_plane_ptf() 

Store  the  plane  data  points  into  the  array  for  display. 

*4t*4i***<«<**4r«<***%4i%i»4<«*****4>***>0«>t>**«4r4<4‘*****4>««*4>*4'*  j 

sci_plane_pir(linc) 

LINE_SEGMENT  *linc; 

( 

int  i; 

double  linc_angle; 

if(line->loc  ==  4)  /*  bottom  sonar  */ 


iine->planc_pts[0j[0J  =  linc->sonar_data_pts{linc->stari_pt_no][OJ  +  offsctlc; 
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Iine->plane_pls[0]ll]  =  line->sonar_data_pts[line->start_pt_no]Il]; 
line->plane_pts[0][2]  =  line->sonar_data_pts[line->start_pt_no][2]  +  ofTsetls; 
line->plane_pis[l][0]  =  line->sonar_daia_pts[line->siari_pt_no][OJ  -  offsctlc; 

linc->planc_plsl  1  ][  1  ]  =  linc->sonar_daia_pisllinc->suiri_pi_no][  1  ]; 
line->plane_pts[l]{2]  =  line->sonar_data_pis[line->siart_pt_no][2]  -  offsetls; 
line->plane_pts[2][0]  =  Iine->sonar_data_pts[line->end_pt_no][0]  +  offset2c; 

line->plane_ptsI2][l]  =  linc->sonar_data_ptsIline->end_pt_no][l]; 
line->plane_pts[2][2]  =  line->sonar_daia_pu[line->end_pt_no][2]  +  offsei2s; 
Iine->plane_pts[3](0]  =  Iine->sonar_data_pts[line->end_pt_no][0]  -  offset2c; 

line->planc_pis[3][l]  =  line->sonar_data_pis[line->end_pi_no][l]; 
line->plane_pls(3][2]  =  line->sonar_data_pis[line->end_pt_no][2]  -  offsei2s; 

}  else 

( 

line->planc_pls[0]10J  =  linc->sonar_daia_pisIlinc->stari_pi_no][0] 

+  offsctlc  *  cos(line->thcta_sonar); 
linc->planc_pts[OJllJ  =  linc->sonar_daia_pis(iine->siart_pt_no][2J 
+  offsetls  +  offsctlc; 

line->plane_pts[0][2]  =  line->sonar_data_pts[line->start_pt_no][l]; 
linc->planc_pts[l][0]  =  linc->sonar_data_pisIlinc->sian_pt_no][0] 

+  offsctlc  *  cos(line->theta_sonar); 
linc->plane_pts(l][ll  =  linc->sonar_data_ptsfIine->start_pt_no]f2] 

-  offsetls  -  offsctlc; 

linc->plane_pts[l][21  =  linc->sonar_data_ptsllinc->start_pt_no]Il]; 
line->plane_pts[2][0]  =  Iine->sonar_data_pis(line->end_pt_no][0] 

+  offset  Ic  *  cos(line->theia_sonar); 
line->planc_pts[2][l]  =  linc->sonar_data_pts[line->end_pt_no][2] 

+  offsct2s  +  offsct2c; 

line->planc_pis[2][2]  =  linc->sonar_data_ptsllinc->end_pt_no][l]; 
linc->plane_pts(3][0]  =  linC'>sonar_data_pts[line->end_pt_no][0] 

+  offsctlc  *  cos(linc->thcta_sonar); 
line->plane_pts[3][l]  =  line->sonar_data_pis[line->cnd_pt_no][2] 

-  offsct2s  -  offsct2c; 

linc->plane_pts[3][2]  =  line->sonar_dala_pts[linc->end_pt_no][l]; 

1 


^  **4i:ti*^itf**^*  *******************  *********************** 

atan20  Not  available  in  GESPAC  math  lib 

**************************************************** j 

double  atan2(x  1  ,y) 
double  xl,y; 

{ 

double  x,xi,  q,  q2; 
int  sign; 

if(y  !=  0.0) 

{ 

X  =  xl/y; 

) 
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{ 

if(xl  >=  0.) 

{ 

retum(O.O); 

) 

else 

{ 

reium(3. 141592654); 

) 

) 

xi  =  aian(x); 

if((xl  <  0.)  &&  (y  >  0.)) 

{ 

xi  =  xi  + 3.141592654; 


if((xl  <  0.)  &&  (y  <  0.)) 

( 

xi  =  xi  +  3.141592654; 

) 


retum(xi); 

) 

j  3tt:(i3(i4t***4‘*«*4t*****:f<4c**4c4c**:((:t‘*»**4c«3t(**3k*****4t«**«****4c* 

initjx)0l() 
void  init_pool() 

I 

/*  pool(i](01  =  r 
pool[i][l]  =  theta 

positions  are  taken  relative  to  start  point  of  AUV  (xjnit,y_init)*/ 

pooIfOJCOJ  =  y_init;  /*  north  wall  -  next  to  launch  pt  */ 
pool[01[l]  =  1.57078; 

pool[l][0]  =  117.58  -  xjnit;  /*  dcccp  end  */ 
pool[l][l]  =  0.0; 

pool(2][0]  =  60.36  -  yjnit; 
pool(2][l]=  1.57078; 

pool[3][0]  =  xjnit; 

pool(3][l]  =  3.141592;  /*shallow  end  */ 

pool[4](0]  =  4.0;  /‘shallow  botom  */ 
pool[4]ll]  =  1.6388; 

poolf5]f0]  =  8.0;  /‘deep  end  */ 
pool[5][l]=  1.57078; 


match_model0 

*»****«**»*****4i**»***»*»4i*****«»******4>********»***^ 

void  malch_niodel(linc) 

L1NE_SEGMENT  ‘line; 

( 

int  i; 

double  delta_r,  dclta_thcia; 
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pooLsurface  =  9;  /*  default  for  no  match  •/ 
for(i  =  0;  i  <  6;  ++i) 

{ 

delia_r  =  line->r_sonar  -  pooifilfOl; 
delia_theia  =  fabs(line->thcia_sonar)  -  poollij[ll; 
while(delta_lheta  >  3.141592) 

{ 

delta_theta  =  delta_theta  -  3.141592; 

) 

if  ((fabs(delta_r)  <=  max_delta_r)  &&  (fabs(della_theta)  <=  max_deita_theta)) 

{ 

pool_surface  =  i; 
return; 

) 

) 

} 

record_line_onO 
void  record_line_on() 

I 

if((outfp2  =  fopcn(“obs.d”,  “w”))  ==  0);  /*  open  file  */ 
exit(-l); 

) 

) 

record_lineO 

void  record_line(linc) 

LINE.SEGMENT  *linc; 

{ 

intij; 

for(i  =0;  i  <  4;  ++i) 

I 

for(j=0;  j  <  3;  ++j) 

{ 

fprintf(outfp2,“%ir,line->plane_pts[i][j]); 
fprinif(outfp2,  “\n”); 

) 

) 

record_line_off() 

•  ******************»»*:tr***»**»*»»»»:»**4^**»****4i*»***  j 

void  record_line_off() 

{ 

fclosc(oulfp2); 

) 
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APPENDIX  C 


Sonar  related  code  for  the  AUV  II  simulator  in  gravy5/auv/magrino/sim. 
Following  is  a  portion  of  the  header  file  AUV.H 


EXT  void  draw_sonar_plot(); 

EXT  void  initialize_sonars(); 

EXT  void  iniiialize_virtual_pool(); 

EXT  void  initialize_pooLobsO; 

EXT  double  offsetlc,  offsetls,  offset2c,  offset2s; 


/*  structure  definitions  */ 


lypedcf  struct  Poini3Siruci  ( /*  3D  point  */ 
double  X,  y,  z; 

}  Points; 

typedef  Points  VeclorS; 

typedef  struct  Matrix4Slruct  ( /*  4  x  4  matrix  */ 
double  elcmcnt[4j[4]; 

)  Mairix4; 

typedef  struct  { 

float  forces[61;/*  summation  of  forces  &  moments  */ 

float  mminv(61f6];  /*  inverse  mass  matrix  */ 

float  acc[6];/*  udot,  vdot,  wdoi  */ 

float  vel[6];/*  u,v,w,p,q,r  */ 

float  pos_change[6]; 

float  delta_t;/*  time  between  updates  */ 

float  pos[3];/*  x,y,z  */ 

float  roll,  pitch,  heading; 

double  commandcd_posturc[15(K)](17);  /*  designated  path  for  temp  use  */ 

double  waypoint[10][4]; 

double  psi_0; 

int  wpts; 

int  obs_avoid; 

int  posturc_no; 

Matrix  H_matrix; 

Matrix  incremental_H_matrix; 

Mauix  T_matrix; 

)  DYNAMIC.STRUC,  •Dyn_ptr; 

typedef  struct  Triangle  ( 

Points  vO; 

Points  V 1 ; 
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PoinG  v2; 

Vectors  normal; 
int il; 
int  i2; 
double  d; 
double  r; 
double  theta; 

)  triangle; 

EXT  triangle  pool[28]; 

typedef  struct 

{  /*  Data  for  each  sonar’s  line  segment  */ 

double  sonar_data_pts[1500](7];/*  x,y,zjangc,coursc,  depth  *1 

double  plane_pis[100][4][3]; 

double  sgmxx, 

sgmyy, 

sgmxy, 

sgmyx, 

sgmx, 

sgmy. 
r_sonar, 
theia_sonar; 
double  mux, 
muy, 

muxx, 
muyy, 

muxy, 
delta_x. 
delta_y, 
sgmx2, 
sgmy2; 

double  sgm_delia_sq, 

sigma, 

d_major, 

d_minor, 

m_major, 

m_minor; 

double  dclta_line, 

start_pt_x, 

start_pt_y, 

end_pt_x, 

end_pt_y; 

double  linejenglh; 

int  i_s, 

j_s. 

n_', 

end_pt_no, 

start_pt_no, 

bad_pt, 

too_long, 

too_far_apart. 
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course_change; 

int  depih_changc, 

range_pt, 

n_plane, 

loc; 

}  L1NE_SEGMENT.  ♦LINE.SEG; 
typedef  struct 

{  /*  Position  and  orientation  of  a  transducer  */ 

Matrix  sonar_matrix; 

Points  *position; 

Vectors  •direction; 

Vectors  *negate_dir; 
double  max.range; 

LINE_SEGMENT  line.data; 

)  TRANSDUCER,  *SONAR_HEAD; 

typedef  struct 

(  /*  Set  of  all  transducers  */ 

int  botiom_on; 
int  right_on; 
int  left_on; 
int  front_on; 

TRANSDUCER  bottom_sonar; 

TRANSDUCER  right_sonar; 

TRANSDUCER  lefl_sonar; 

TR.\NSDUCER  front_sonar; 

TRANSDUCER  top.sonar; 

)  SONAR.SUITE,  *SONAR_PTR; 

typedef  struct  { 

float  bottom_clearance; 
float  gravity;/*  acceleration  due  to  gravity  */ 
float  rho;  /*  water  density  of  environment  */ 
float  nu;  /*  water  viscosity  of  environment  */ 

AUTOPILOT_STRUCT  aulop: 

OBSERVER  obs; 

VEH1CLE_GE0METRY  geo;  /*  vehicle  geometry  struct  */ 
FLAGS  sys;  /*  flags  struct  */ 

COEFFICIENTS  coeff;  /*  hydro  coefficients  struct*/ 
MBARI.BAY  bay; 

DYNAM1C_STRUC  dyn;/*  ivchicle  position  struct  */ 
AUV_POLYGONS  poly;/*  auv  objects  (polygons)  */ 
SURFACES  surf: 

RECORDER  rec; 

NPS_POOL  pool; 

PANELS  panel; 

SEMAPHORES  multi; 

CONSTRAINTS  consuaint; 

SONAR_SUITE  SONAR; 
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double  from_range; 


)  Submarine,  *Sub_pu; 


Sonar  specific  constants/variables  found  in  SONAR.H 

#include  <sulio.h> 

#include  <maih.h> 

/*  Constants  for  sonar  package  */ 

#define  MAX_LINE_LENGTH  200.0 
#define  MAX_PT_GAP  300.0 
#dcfine  MAX_COURSE_CHANGE  10.0 
#define  MAX_SONAR_RANGE  1 100.0 
#define  MAX_DEPTH_CHANGE  20.0 
#definc  MIN_NO_PTS  3 
#define  MAX_RAND  37767.0 
^define  SQR(x)  ((x)  •  (x)) 

#dcfine  CONVERT_TO_INCHES  .2879 

/*  Constants  for  line  seg  package  */ 

#dcfine  cl  3.0  /*  sigma  factor  */ 

^define  c2  .6  /*  deviation  const  */ 

#define  c3  .60  /♦  ellipse  thinness  ratio  */ 

#dcfine  c4  24.0  /*  minimum  line  segment  length  */ 

typedef  su'uct  /*  ray-uacing  su-ucturc  */ 

{ 

double  ray_matrix(1001(81; 
intray_matrix_index: 

Vcctor3  *ray_origin; 

Vector3  *ray_direction; 

)S0NAR_RAY,  *RAY_STRUCT; 

AUV  SIMULATOR  SONAR  SIMULATOR 

This  package  is  called  from  main_loop.c  of  auv  each  time  that  the  auv  position  is  updated.  The  procedure 
initialize_sonars()  is  called  from  the  iniualizc_auv()  of  initialize.c,  it  sets  all  of  the  sonars  to  their  initial  po¬ 
sitions  and  orientations.  The  procedure  .sonar_rangc  performs  a  ray-trace  routine  to  determine  if  a  sonar  beam 
has  intersected  with  a  world  polygon  in  a  manner  to  give  a  valid  sonar  return.  Sonar_range  sends  a  valid  range 
point  to  the  package  line_scg_bay.c  for  Iea.st-squares  fitting,  and  determination  of  a  plane  through  the  line  that 
has  been  fit  to  the  data  points.  The  procedure  plot_sonar0  displays  the  range  points  and  the  planes  in  the  pool.- 
Constants  are  contained  in  the  sonar.h  header  file,  and  structures  arc  in  the  auv.h  file. 

#include  <riiath.h> 

#include  <stdio.h> 

#includc  <malloc.h> 
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#include  <sys/lypes.h> 


#include  “auv.h” 

#include  “sonar.h” 

^define  NEWTYPE(x)  (x  *)(malloc((unsigned)sizeof(x))) 

/***  function  declarations  ******/ 

double  V3DotO; 

Veclor3  •V3NewO: 

Vector3  *V3Negatc(); 

Vector3  *V3Duplicate(): 
double  V3LengthO: 
double  V3SquarcdLength(); 

Vector3  *V3NonnalizcO; 

Vector3  *V3MullByScalarO; 

Vector3  *V3DivideByScalar(); 

Vccior3  *V3SubO; 

Veclor3  *V3AddO: 

double  V3DistanccBctwecnTwoPoints(V. 

void  V3MullMalrixByPoini(); 

void  reset_line(); 

void  check_length(); 

void  chcck_dislancc(): 

void  chcck_coursc(); 

void  check_depth(): 

void  ping_sonar(): 

void  trace_ray(): 

Vcctor3  *computc_rcflcciionO; 
void  get_range(); 

double  compuie_distance_from_sonarO; 

void  inilialize_ray_sirucl(); 

void  position_sonar(); 

void  direct_sonar(); 

void  replay_sonar_data(); 

int  maich_modcl(); 

I  }tiH,^^m’$»***»*****  ****************************  ******** 

initial  izc_sonarsO 

Set  up  the  individual  sonar  head  directions,  locations,  and  ranges. 

void  initialize_sonars(  SONAR_AUV) 

SONAR_SUITE  *SONAR_AUV; 

{ 

int  i,  j,k; 

SONAR.  AUV->bottom_on  =  TRUE; 

SONAR_AUV->bottom_sonar,dircclion  =  V3New(0.0,  -1.0, 0.0); 
SONAR_AUV->bottom_sonar.position  =  V3New(0.0,  -1.0, 0.0); 
SONAR_AUV->bottom_sonar.line_data.looJong  =  FALSE; 
SONAR_AUV->bottom_sonar.linc_data.loo_far_apan  =  FALSE; 
SONAR_AUV->boltom_sonar.linc_data.coursc_changc  =  FALSE; 
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SONAR_AUV->bollom_sonar.line_(laia.dcpih_change  =  FALSE; 

SONAR_AUV->righl_on  =  TRUE; 
SONAR_AUV->righl_sonar.dircciion  =  V3New(1.0, 0.0, 0.0); 
SONAR_AUV->righl_sonar.po.siiion  =  V3Ncwv0.0,  -1.0,  0.0;, 
SONAR_AUV->right_sonar.line_daia.loo_long  =  FALSE; 
SONAR_AUV->righi_sonar.line_data.too_far_apart  =  FALSE; 
SONAR_AUV->right_sonar.line_daia.course_change  =  FALSE; 
SONAR_AUV->righi_sonar.linc_dala.depth_change  =  FALSE; 

SONAR_AUV->left_on  =  TRUE; 

SONAR_AUV->left_sonar.direclion  =  V3New(-L0, 0.0, 0.0); 
SONAR_AUV->lefl_sonar.position  =  V3New(0.0,  -1.0, 0.0); 
SONAR_AUV->lefl_sonar.line_data.looJong  =  FALSE; 
SONAR_AUV->left_sonar.line_daia.ioo_far_apart  =  FALSE; 
SONAR_AUV->lcfl_sonar.linc_daia.course_change  =  FALSE; 
SONAR_AUV->lcft_sonar.line_data.dcpth_changc  =  FALSE; 

SONAR_AUV->froni_on  =  TRUE; 
SONAR_AUV->fronl_sonar.direciion  =  V3New(0.0, 0.0,  -1.0); 
SONAR_AUV->front_sonar.posilion  =  V3Ncw(0.0,  -1.0, 0.0); 
SONAR_AUV->front_sonar.line_data.loo_long  =  FALSE; 
SONAR_AUV->front_sonar.line_dala.loo_far_apart  =  FALSE; 
SONAR_AUV->froni_sonar.line_data.coursc_changc  =  FALSE: 
SONAR_AUV->froni_sonar.linc_data.dcpth_change  =  FALSE; 

SONAR_AUV->boltom_sonar.negaie_dir=  V3New(0.0, 1.0, 0.0); 
SONAR_AUV->righl_sonar.negaie_dir  =  V3New(-1.0, 0.0, 0.0); 
oONAR_AUV->lefl_sonar.negate_dir  =  V3Ncw(1.0, 0.0, 0.0); 
SONAR_AUV->front_sonar.ncgaic_dir  =  V3Ncw(0.0, 0.0,  1 .0); 

SONAR_AUV->bottom_sonar,position  =  V3New(0.0, 0.0,  0.0); 
SONAR_AUV->righl_sonar.posiiion  =  V3Ncw(0.0, 0.0, 0.0); 
SONAR_AUV->lcfLsonar.posiuon  =  V3Ncw(0.0, 0.0, 0.0); 
SONAR_AUV->froni_sonar.posiiion  =  V3Ncw(0.0, 0.0, 0.0); 

SONAR_AUV->bouom_sonar.linc_daia.loc  =  1; 
SONAR_AUV->righi_sonar.line_dala.ioc  =  2; 
SONAR_AUV->Iefl_-sonar.linc_data.Ioc  =  3; 
SONAR_AUV->front_sonar.line_daia.Ioc  =  4; 

SONAR_AUV->boUom_sonar.max_rangc  =  MAX_SONAR_RANGE; 
SONAR_AUV->righi_sonar.max_rangc  =  MAX_SONAR_RANGE; 
SONAR_AUV->left_sonar.max_range  =  MAX_SONAR_RANGE; 
SONAR_AUV->fronl_sonar.max_rangc  =  MAX_SONAR_RANGE; 

for  0=0;  i<  100;  ++i) 

( 

SONAR_AUV->boilom_sonar.linc_data.planc_pts[i][0][0]  =  0.0; 
SONAR_AUV->righi_sonar.Iinc_data.planc_pls(i][0][0]  =  0.0; 
SONAR_AUV->lcfl_sonar.linc_daia.planc_pis[i]10][0]  =  0.0; 
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SONAR_AUV->front_sonar.line_daia.planc_pisli)lO]10]  =  0.0; 


initialize_ray_strucl() 

void  iniualize_ray_slruct(ray_struct) 

SONAR_RAY  *ray_struct: 

( 

int  ij; 

for(i  =  0;  i  <  100;  ++i) 

{ 

for(j  =  0;  j  <  8;  ++j) 

1 

ray_siruci->ray_malrix[il[jl  =  0.0; 

) 

) 


ray_siruct->ray_malrix_index  =  0; 
ray_slrucl->ray_origin  =  V3Ncw(0.0, 0.0, 0.0); 
ray_sirucl->ray_direciion  =  V3New(0.0, 0.0, 0.0); 


*  draw_sonar_plol 

*  Select  each  sonar  in  sequence,  check  for  an  echo,  then  plot  data. 

* 

*♦**•*****•***»*•»**»*•*»♦*»*»»*♦♦»•»»»*»»*♦»•**♦***  j 

void  draw_sonar_plol(SONAR_AUV,  auv) 

SONAR.SUITE  *SONAR_AUV; 

Sub_ptr  auv; 

( 

SONAR_HEAD  which_sonar; 

LINE_SEG  line_vars; 

volatile  SONAR_RAY  ray_daia: 

volatile  SONAR_RAY  *'ray_sU'uci; 

ray_struct  =  &ray_daia; 

which_sonar  =  &SONAR_AUV->lcfLsonar;  /*  used  for  mission  rcplay-in.sert  desired  sonar*/ 
if  (auv->con.straint.box)  /*  actual  mission  replay  switch  •/ 

( 

replay_sonar_data(which_sonar,  auv); 


else 

{ 

initialize_ray_struc  t(ray_struc  t) ; 
which_sonar  =  &SONAR_AUV->bollom_sonar; 
if  (SONAR_AUV->botlom_on) 
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ping_sonar(which_sonar,  auv,  ray_siruci); 

++ray_siruci->ray_mairix_index; 
which_sonar->direction->y  =  -0.99619; 
which_conar->direclion->x  =  0.07547; 
whii;h_sonar->direcLion->/.  =  U.U4357, 
ping_sonar(which_sonar,  auv,  ray_suua); 
++ray_struct->ray_matrix_index; 
which_sonar->direclion->y  =  -0.99619; 
which_sonar->dircclion->x  =  0.07547; 
which_sonar->direclion->z  =  -0.04357; 
ping_sonar(which_sonar,  auv,  ray_siruct); 
++ray_slruct->ray_malrix_indcx; 
which_sonar->dircclion->y  =  -0.99619; 
which_sonar->direcUon->x  =  0.0; 
which_sonar->direction->z  =  -0.08715; 
ping_sonar(which_sonar,  auv,  ray_strucl); 
-‘•+ray_struct->ray_mauix_indcx; 
which_sonar->dircclion->y  =  -0.99619; 
which_sonar->dir‘X'tion->x  =  -0.07547; 
which_sonar->dirccUon->z  =  -0.04357; 
ping_sonar(which_sonar,  auv,  ray_siruci); 
++ray_siruct->ray_mairix_index; 
which_sonar->dircction->y  =  -0.99619; 
which_.sonar->dircclion->x  =  -0.07547; 
which_sonar->direcLion->z  =  0.04357; 
ping_sonar(,which_sonar,  auv,  ray_strucu; 
++ray_slruct->ray_malrix_indcx; 
which_sonar->direction->y  =  -0.99619; 
which_sonar->dirccuon->x  =  0.0; 
which_sonar->dirccuon->z  =  0.08715: 
ping_sonar(which_sonar,  auv,  ray_slrucl); 
++ray_slruct->ray_malrix_index; 


which_sonar  =  &SONAR_AUV->righl_sonar; 
if  (SONAR_AU  V->right_on) 

( 

ping_sonar(which_sonar,  auv,  ray_suuct); 
r+ray_siruct->ray_malrix_index; 
which_sonar->dircclion->x  =  0.99619; 
which_sonar->direction->y  =  0.07547; 
which_sonar->dirccuon->z  =  0.04357; 
ping_sonar(which_sonar,  auv,  ray_siruct); 
++ray_siruci->ray_matrix_index; 
which_sonar->dircclion->x  =  0.99619; 
which_sonar->dirccUon->y  =  0.07547; 
which_sonar->dircclion->z  =  -0.04357; 
ping_sonar(which_sonar,  auv,  ray_sirucl); 
++ray_siruct->ray_mairix_indcx; 
which_.sonar->dircctjon->x  =  0.99619; 
which_sonar->dirccuon->y  =  0.0; 
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which_sonar->direcuon->z  =  -0.08715; 
ping_sonar(which_sonar,  auv,  ray_strucO; 
++ray_siruci->ray_mairix_indcx; 
which_sonar->direction->x  =  0.99619; 
which_sonar->(lircciion->>  =  -0.07547; 
which_sonar->direcUon->z  =  -0.04357; 
ping_sonar(which_sonar,  auv,  ray_struct); 
++ray_stnict->ray_matrix_indcx; 
which_sonar->direcUon->x  =  0.99619; 
which_sonar->direction->y  =  -0.07547; 
which_sonar->direction->z  =  0.04357; 
ping_sonar(which_sonar,  auv,  ray_struct); 
++ray_strucl->ray_matrix_indcx; 
which_sonar->direction->x  =  0.99619; 
which_sonar->direcuon->y  =  0.0; 
which_sonar->dircction->z  =  0.08715; 
ping_sonar(which_sonar,  auv,  ray_siruci); 
++ray_siruci->ray_matrix_indcx; 

} 

which_sonar  =  &SONAR_AUV->lcfl_sonar; 

if  (SON  AR_AU  V->left_on) 

{ 

ping_sonar(which_sonar,  auv,  ray_struct); 
++ray  _su'ucl->ray_matri  x_indcx ; 
which_sonar->direcuon->x  =  -0.99619; 
which_sonar->direction->y  =  0.07547; 
which_sonar->direcuon->z  =  0.04357; 
ping_sonar(which_sonar,  auv,  ray_struct); 
++ray_slruct->ray_malrix_indcx; 
which_sonar->direcuon->x  =  -0.99619; 
which_sonar->direction->y  =  0.07547; 
which_sonar->direciion->z  =  -0.04357; 
ping_sonar(which_sonar,  auv,  ray_siruci); 
++ray_struct->ray_tnairix_index; 
which_sonar->direciion->x  =  -0.99619; 
which_sonar->dircction->y  =  0.0; 
which_sonar->direcuon->z  =  -0.08715; 
ping_sonar(which_sonar,  auv,  ray_sirucl); 
++ray_suucl->ray_mairix_index; 
which_sonar->direction->x  =  -0.99619; 
which_sonar->dirccuon->y  =  -0.07547; 
which_sonar->direclion->z  =  -0.04357; 
ping_sonar(which_sonar,  auv,  ray_struct); 
++ray  _strucl->ray_mairi  x_  index ; 
which_sonar->direcuon->x  =  -0.99619; 
which_sonar->direcuon->y  =  -0.07547; 
which_sonar->direcuon->z  =  0.04357; 
ping_sonar(which_sonar,  auv,  ray_siruct); 
++ray_struct->ray_mairixjndex; 
which_sonar->dircciion->x  =  -0.99619; 
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which_sonar->(lirection->y  =  0.0; 
which_sonar->direction->z  =  0.08715; 
ping_sonar(which_sonar,  auv,  ray_struci); 
++ray_struct->ray_mairix_index; 

) 

which_sonar  =  &SONAR_AUV->front_sonar; 

if  (SONAR_AUV->froni_on) 

{ 

ping_sonar(which_sonar,  auv,  ray_sirucl); 
++ray_struci->ray_malrix_index; 
which_sonar->direclion->z  =  -0.9%19; 
which_sonar->direction->y  =  0.07547; 
which_sonar->direcuon->x  =  0.04357; 
ping_sonar(whjch_sonar,  auv,  ray_suuct); 
++ray_siruct->ray_mauix_indcx; 
which_sonar->direcuon->z  =  -0.9%19; 
which_sonar->direciion->y  =  0.07547; 
which_sonar->dircclion->x  =  -0.04357; 
ping_sonar(which_sonar,  auv,  ray_struct); 
++ray_sirucl->ray_matrix_index; 
which_sonar->direclion->z  =  -0.99619; 
which_sonar->direciion->y  =  0.0; 
which_sonar->dircction->x  =  -0.08715; 
ping_sonar(which_sonar,  auv,  ray_suuci); 
++ray_struct->ray_mauix_indcx; 
which_sonar->direction->z  =  -0.99619; 
which_sonar->direcUon->y  =  -0.07547; 
which_sonar->direcuon->x  =  -0.04357; 
ping_sonar(which_sonar,  auv,  ray_sirucl); 
++ray_su-ucl->ray_niauix_index; 
which_sonar->direcuon->z  =  -0.9%19; 
which_sonar->direclion->y  =  -0.07547; 
which_sonar->direcuon->x  =  0.04357; 
ping_sonar(which_sonar,  auv,  ray_sUTict); 
++ray_struct->ray_matrix_index; 
which_sonar->dirccUon->z  =  -0.99619; 
which_sonar->direclion->y  =  0.0; 
which_sonar->dircciion->x  =  0.08715; 
ping_sonar(which_sonar,  auv,  ray_struci); 
++ray_strucl->ray_matrix_index; 

1 


which_sonar  =  &SONAR_AUV->bouom_sonar; 
which_sonar->direcUon->x  =  0.0; 
which_sonar->direc6on->y  =  -1.0; 
which_sonar->dircction->z  =  0.0; 
direct_sonar(which_sonar,auv); 
if  (SONAR_AUV->boitom_on) 

{ 

gei_range(which_sonar,auv,  ray_struci); 
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) 


which_sonar  =  &SONAR_AUV->right_sonar; 
which_sonar->direction->x  =  1 .0; 
which_sonar->dircciion->y  =  0.0, 
whicu_sonar->direclion->z  =  0.0; 
direct_sonar(which_sonar,auv); 

if  (SONAR_AUV->righLon) 

gct_range(which_sonar,auv,  ray  .struct); 

) 


which.sonar  =  &SONAR_AUV->left_sonar; 
which_sonar->direction->x  =  -1.0; 
which_sonar->dircction->y  =  0.0; 
which_sonar->direction->z  =  0.0; 
direct_sonar(which_sonar,auv); 

if  (SONAR.AU  V->left_on) 

( 

get_rangc(which_sonar,auv,  ray  .struct); 

) 


which.sonar  =  &SONAR_AUV->fronl.sonar; 
which.sonar->direction->x  =  0.0; 
which_sonar->direction->y  =  0.0; 
which.sonar->direction->z  =  -1.0; 
direct.sonarf  which.sonar, auv); 
if  (SONAR.AUV->front_on) 

( 

get  range(which_sonar,auv,  ray.struct); 

) 


free(ray_struct->ray  .origin); 
free(ray.struct->ray  .direction); 

reset.sonars(SONAR.AUV);/*  Back  to  orig  posits  */ 
}  /*  end  else*/ 

) 


plot.sonarO 

Draw  range  marks  and  derived  planes.  From  sonar  shows  range  only. 
******»*****♦♦*♦****•*♦****»*»♦*»*♦»»♦♦**»♦*»♦*»**** y 
plot.sonar(line,auv) 

LINE.SEGMENT  *line; 

Sub.ptr  auv; 

( 

float  tempi  (3J,  temp2[3],  temp3[3],  temp4[3]; 
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int  ii; 

/*  Plot  the  range  points  */ 

for  (fi  =  0;  fi  <  1500;  fi  =  ++fi) 

I 

if  (line->sonar_data_pts[fi][3]  !=  0.0) 

{ 

if  ((line->loc  —  1)  II  (line->loc  =3)) 

ctnov(line->sonar_data_pts(fil[0],  linc->sonar_(iata_pus[fi]l  1  ], 
line->sonar_data_pts[fi}[2]); 
else 


/* 

*  y  &  z  points  swapped  by  line_seg_bay 

*  routines,  so  swap  back 


if(line->sonar_data_pLsffi][3]  ==  1.0) 

I 

RGBcolor(200.0,200); 
cmovOine->sonar_data_pts[fi](0], 
line->sonar_data_pts[fi][  1 1, 
line->sonar_data_pts[fi][2]);) 
else 


cmov(line->sonar_data_pts(fi](0], 
line->sonar_data_pts[fi]  [2] , 
line->sonar_data_pts[fi][l]); 
switch(line->loc) 

{ 

case  1; 

RGBcolor(255, 255, 0); 
break; 

case  2: 

RGBcolor(0, 255, 0); 
break; 

case  3: 

RGBcolor(0. 0,255); 
break; 

case  4; 

RGBcolor(255, 0, 0); 
break; 

) 

) 

charstr(‘‘x”); 

if(auv->constraint.box) 

{ 
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cmov(line->sonar_dala_pis[fi][3],  line->sonar_dala_pls[fi]l4], 
line->sonar_data_pts[  fi  ]  [  5  ] ); 

charslr(“.”): 


} 

if(auv->constraint.snake)  /*  LOS  guidance  path  simulator  selection*/ 

{ 

cmov(auv->dyn.waypoint[auv->dyn.posture_no](l], 

auv->dyn.waypoint[auv->dyn.posture_no][2], 

-auv->dyn.waypoint[auv->dyn.posture_no][0]); 

charstrC'W”); 

) 

/*  Plot  the  generated  planes,  sides  in  white,  bottom  in  black  */ 
for  (fi  =  0;  fi  <  100;  ++fi) 

{ 

if  (linc->plane_pts[fi][0][0]  !=  0.0) 

{ 

if  (line->loc  ==  1 ) 

{ 

RGBcolor(0,  0, 0); 


)  else 


RGBcolord  0,200,  100); 


) 

tempi  [0]  ■-  (float)  line->plane_pts(fi][0](0]; 
ternpUl]  =  (float)  linc->plane_pts[fi][0](l]; 
tempi  [2]  =  (float)  line->planc_pts(fi][0][2]; 
temp2[0]  =  (float)  line->plane_pts(fi][l](0]; 
temp2[l]  =  (float)  line->plane_pts(fi][l)[l]; 
temp2[2]  =  (float)  line->planc_pts[fi][l](2]; 
temp3[0]  =  (float)  line->plane_pts[fi][2](0]; 
temp3[l]  =  (float)  line->plane_pts[fi][2][l]; 
tcmp3[2]  =  (float)  line->plane_pts[fi][2][2]; 
tcmp4[0]  =  (float)  linc->planc_pis[fi]f3](01; 
temp4[l]  =  (float)  line->plane_pts[fi][3](l]; 
temp4[2]  =  (float)  linc->planc_pts[fi][3][2]; 

bgnpolyconO; 

v3f(templ); 

v3f(temp2): 

v3f(temp4); 

v3f(temp3); 

endpolygonO; 


) 
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seleciJlO  &  seleci_i20 

Returns  the  appropriate  value  for  use  in  the  point-in-polygon  test. 

selectJUa,  b,  k) 

double  a,  b; 

intk; 

{ 

switch  (pool[k].il) 

{ 

case  0: 

return  (a); 
break; 

case  1: 

return  (b); 
break; 

) 

) 

select_i2(a,  b,  k) 
double  a,  b; 
int  k; 

{ 

switch  (pool[k].i2) 

{ 

case  1; 

return  (a); 
break; 

case  2; 

return  (b); 
break; 

} 

) 


reset_sonars() 

Used  to  return  sonars  to  original  values  for  next  cycle. 
*♦•♦♦♦***♦♦*****♦♦*♦♦♦*♦****»»**♦*♦**»♦♦***♦**»•»♦**  / 


reset_sonars(SONAR_AUV) 

SONAR.SUITE  *SONAR_AUV; 

{ 

int  i  j; 

free(SONAR_AUV->bottom_sonar.direction); 

free(SONAR_AUV->right_sonar.direction); 

free(SONAR_AUV->left_sonar.direction); 

free(SONAR_AUV->front_sonar.direction); 

free(SONAR_AUV->bottom_sonar.negate_dir); 

free(SONAR_AUV->right_sonar.negatc_(lir); 
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free(SONAR_AUV->left_sonar.negaU5_dir); 

free(SONAR_AUV->front_sonar.negate_dir); 

free(SONAR_AUV->boiU)m_sonar.posiiion); 

free(SONAR_AUV->right_sonar.posilion); 

frcc(S0N.4R_AU  V->lcl  l_sonar.posilion;; 

free(SONAR_AUV->from_sonar.posiUon); 

SONAR_AUV->boltom_sonar.direclion  =  V3Ncw(0.0,  -10, 0.0); 
SONAR_AUV->boitom_sonar.linc_data.tooJong  =  FALSE; 
SONAR_AUV->boltom_sonar.line_data.too_far_apart  =  FALSE; 
SONAR_AUV->bottom_sonar.line_data.course_change  =  FALSE; 
SONAR_AUV->bottom_sonar.line_data.deplh_change  =  FALSE; 

SONAR_AUV->right_sonar.direcUon  =  V3New(L0, 0.0, 0.0); 
SONAR_AUV->right_sonar.line_data.too_long  =  FALSE; 
SONAR_AUV->right_sonar.linc_dala.too_far_apart  =  FALSE; 
SONAR_AUV->righl_sonar.line_daia.course_change  =  FALSE; 
SONAR_AUV->right_sonar.linc_data.dcpth_change  =  FALSE; 

SONAR_AUV->lefl_sonar.direction  =  V3New(-L0, 0.0, 0.0); 
SONAR_AUV->lcfLsonar.linc_data.looJong  =  FALSE; 
SONAR_AUV->iefl_sonar.line_data.loo_far_aparl  =  FALSE; 
SONAR_AUV->left_sonar.line_data.course_change  =  FALSE; 
SONAR_AUV->lcfLsonar.linc_data.dcpth_changc  =  FALSE; 

SONAR_AUV->froni_sonar.direclion  =  V3Ncw(0.0, 0.0,  -1.0); 
SONAR_AUV->front_sonar.line_data.too_long  =  FALSE; 
SONAR_AUV->front_sonar.line_data.too_far_apart  =  FALSE; 
SONAR_AUV->froni_sonar.line_data.course_change  =  FALSE; 
SONAR_AUV->front_.sonar.line_data.dcpth_changc  =  FALSE; 

SONAR_AUV->bottom_sonar.negaie_dir  =  V3New(0.0, 1 .0, 0.0); 
SONAR_AUV->right_sonar.negate_dir  =  V3Ncw(-1.0, 0.0, 0.0); 
SONAR_AUV->lefl_sonar.negaie_dir  =  V3Ncw(1.0, 0.0, 0.0); 
SONAR_AUV->froni_sonar.negate_dir  =  V3New(0.0, 0.0, 1.0); 
SONAR_AUV->boUom_sonar.posiiion  =  V3New(0.0,  1.0, 0.0); 
SONAR_AUV->right_sonar.position  =  V3Ncw(-L0, 0.0,  0.0); 
SONAR_AUV->left_sonar.posilion  =  V3Ncw(L0, 0.0, 0.0); 
SONAR_AUV->froni_sonar.position  =  V3Ncw(0.0, 0.0,  1.0); 

} 

^ ♦**♦*****»*****♦*****♦♦**»*♦**»**♦»*♦*»****♦♦***♦♦♦** 
position_sonar() 

Using  the  auv  H.  matrix,  move  the  sonars  to  the  nose. 
♦♦**»♦♦************♦»♦♦♦*****♦***»**»»♦**♦♦♦»♦*♦**»*/ 
void  position_sonar(sonaf,  auv) 

TRANSDUCER  *sonar; 

Submarine  *auv; 

{ 

Vector3  *tcmp_pi; 
inti,j; 
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pushmatrixO; 

loadmatrix(auv->dyn.H_matrix); 

lranslate(0.0, 0.0,  -32.0;; 

getniatrix(sonar->sonar_matrix); 

popmatrixO: 

sonar->posilion->x  =  sonar->sonar_malrix[3](0]; 
sonar->posiiion->y  =  sonar->sonar_matrix[3][l]; 
sonar->posilion->z  =  sonar->sonar_matrix[3][2]; 

} 

y4r4i****4t4<*4<*«4<>*i4<****4i*4<*4‘*>t<***4‘*4t»*4t«*****»:fc4c«**4[***«« 

Orient  the  sonars  depending  on  heading,  pitch,  roll. 

*«***»4ci|i:|i*i^4i»****4t********»»****  A*********  ********  »*^ 


void  direct_sonar(sonar,  auv) 

TRANSDUCER  *sonar; 

Sub_ptr  auv; 

( 

double  temp; 

double  alpha  =  auv->dyn.heading  ♦  DEG_TO_RAD; 
pushmatrixQ; 

loadmatrix(auv->dyn.H_maU'ix); 

getmatrix(sonar->sonar_matrix); 

popmatrixO; 

switch  (sonar->linc  data.loc) 

{ 

case  T. 

V3MultMatrixByPoint(sonar); 
sonar->direciion  =  V3Normalize(sonar->direction); 
sonar->negate_dir->x  =  -sonar->direction->x; 
sonar->ncgalc_dir->y  =  -sonar->dircciion->y; 
sonar->negate_dir->z  =  -sonar->direction->z; 
break; 

case  2: 


V3MultMatrixByPoint(sonar); 
sonar->direction  =  V3Nonnalize(sonar->direction); 
sonar- >dircclion->z  =  -sonar->direction->z; 
sonar->negate_dir->x  =  -sonar->dircclion->x; 
sonar->negate_dir->y  =  -sonar->direction->y; 
sonar->negate_dir->z  =  -sonar->direction->z; 
break; 

case  3: 
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V3MuUMatrixByPoint(sonar); 
sonar->direciion  =  V3Normalize(sonar->direclion); 
sonar->direclion->z  =  -sonar->direcuon->z; 
sonar->negate_dir->x  =  -sonar->dircction->x: 
sonar->negaic_dir->y  =  -sonar->dircciion->y; 
sonar->negaie_dir->z  =  -sonar->direcuon->z; 
break; 

case  4: 

V3MuItMatrixByPoint(sonar); 
sonar->direclion  =  V3Normalize(sonar->direction); 
sonar->negate_dir->x  =  -sonar->direciion->x; 
sonar->negaie_dir->z  =  -sonar->direclion->z; 
sonar->negate_dir->y  =  -sonar->direclion->y; 
break; 

case  5: 

sonar->direclion->x  =  sin(auv->dyn.pilch  *  DEG_TO_RAD)  *  cos(alpha); 
sonar- >dircclion->y  =  (fabs(cos(auv->dyn.pilch  *  DEG_TO_RAD))); 
sonar->direclion->z  =  -(sin(auv->dyn.pitch  *  DEG_TO_RAD)  *  sin(alpha)); 
sonar->direciion  =  V3Normalize(sonar->direciion); 
sonar->ncgate_dir->x  =  -sonar- >direclion->x; 
sonar->ncgaie_dir->y  =  -sonar->dircciion->y; 
sonar->negaic  dir->z  =  -sonar->dircciion->z; 

) 

) 

resei_lineO 

m*******’¥******»i**************:*»*******************»  f 

void  reset_line(linc,  closcsl_pl,  min_rangc,  auv) 

LINE_SEGMENT  *iine; 

Point3  *cJosest_pt; 
double  min_range; 

Sub_pir  auv; 

{ 

line->n_s  =  0; 
line->i_s  =  0; 
line->start_pt_no  =  0; 

Iine->end_pt_no  =  0; 
linc->range_pl  =  0; 

Hne->sonar_data_pis[0][0]  =  closest_pt->x; 
linc->sonar_data_pls(0][l]  =  closcst_pi->y; 
line->sonar_data_pls[0]l2]  =  closest_pi->z; 
line->sonar_daui_pts[0][3)  =  min_rangc; 
line->son,  r_data_pts[0][4]  =  auv->dyn.heading; 
line->sonar_data_pts(0][5]  =  auv->dyn.H_mairix{3I[l]; 

) 

/  ^itiii>i^^^m********m**^** ********* ********i¥*****^‘******** 
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checkJengthO 

void  check_lenglh(line) 

LINE_SEGMENT  *line; 

{ 

line->line_length  =  fsqrt(SQR(line->sonar_data_pts[line->start_pt_no][0]  - 
Iine->sonar_dala_pts[linc->cnd_pt_no][0])  + 
SQR(linc->sonar_data_pis[linc->siart_pt_no][l  ]  - 
line->sonar_data_pu[line->end_pl_no](  1  ])  + 
SQR(line->sonar_data_pts[linc->siart_pt_no]  [2]  - 
line->sonar_data_pis[linc->end_pt_no](2])): 
if(linc->line_iength  >  MAX_LINE_LENGTH) 
line->too_long  =  TRUE; 

) 


^  *******’¥*************  m**^*^^************ 

chcck_distanceO 

void  check_distance(linc,  closcst_pt) 

LINE_SEGMENT*linc: 

Poini3  *closcsl_pi; 

( 

double  distancc_hciwccn_pLs; 


if  (line->loc  =*  1) 

distancc_bctwecn_pis  =  fsqri(SQR(linc->sonar_daia_pis[line->end_pi_no][0]  - 

closest_pi->x)  +  SQRO*ne->sonar_data_pts[line->end_pLno][  1  ] 
closcsi_pi->y)  +  SQR(linc->sonar_daia_pis[Iinc->cnd_pi_no]|21 
closcsLpl->z)); 

else 

disuince_bclwcen_pts  =  fsqri(SQR(linc->sonar_data_pisllinc->end_pt_no][0]  - 

closcsl_pi->x)  +  SQR(linc->sonar_data_pis[line->cnd_pl_no][l] 
closcsi_pt->z)  +  SQR(linc->sonar_daia_pis[line->cnd_pl_noJ[2] 
closest_pi->y )); 

if  (disiancc_bciwccn_pLs  >  MAX_PT_GAP) 

I 

linc->loo_far_apart  =  TRUE: 

) 


check_courseO 

4i^4i4i4t4t*4>**4>4'****^^**#<*4'%4(*4'**3ti***4<4t***4'******3<<#  +  ****  j 

void  check_course(linc,  auv) 

LINE_SEGMENT  *linc; 

Sub_ptr  auv; 


double  hcading_diffcrcnce; 
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heading_dilferencc  =  line->sonar_dala_pis[Iinc->siart_pi_nol[4]  -  auv->dyn.hcading 
if  (heading_difference  <  0.0) 

heading_difTcrence  =  heading_djffercncc  +  360.0; 
if  0'^dinj.'_dillcrcntc  >  18U.0) 

heading_difference  =  360.0  -  hcading_difference; 

if  (hcading_diffcrcnce  >  MAX_COURSE_CHANGE) 
linc->coursc_changc  =  TRUE; 

) 


^  ^^m**m**********m*******iii***i$*****i¥*** *************** 

check_depih() 

void  check_dep(h(line,  auv) 

LlNE_SEGMENT*linc; 

Sub_ptr  auv; 

( 

ifUabs(.linc->sonar_data_pLs[line->siari_pi_no|151  -  auv->dyn.H_n>ain\131i  1  b  > 
MAX_DEPTH_CHANGE) 
linc->dcplh_changc  =  TRUE; 


^  «**««* *41 »«««*« 4i4t4i4t4> 

ping_sonar() 

****•»*»»*•»•««•»»»•»»«*»***»*»»:).  ***»*«»»*»*«»«**»»»  y 

void  ping_sonar(sonar,  auv,  ray_siruct) 

TRANSDUCER  *sonar; 

Sub_pir  auv; 

SONAR  RAY  •ray_sU’ucl; 

( 

direcl_sonar(sonar,  auv); 
posilion_sonar(sonar,auv); 

ray_struct->ray_dircclion->x  =  sonar->dirccUon->x; 
ray_slruci->ray_dircclion->y  =  sonar->dircction->y; 
ray_siruct->ray_dirccuon->z  =  sonar->dircclion->z; 
ray_slruci->ray_origiii->x  =  sonar->fX)siUon->.x; 
ray_siruci->ray_origin->y  =  sonar->posiiion->y; 
ray_struct->ray_origin->z  =  sonar->posi(ion->z; 
lrace_ray(ray_slruci,  sonar); 

1 


y4i**«***4t«*****»4i****4>*4>******»«4>*4i4>****4i****4t»*4t***** 

trace_rayO 

**4>*4>*4i*4i**4i*****4r****4<***4'*4r****4>*************4'**>t>*  j 

void  trace_ray(ray_suaici,  sonar) 

SONAR_RAY  •ray_struct; 

TRANSDUCER  ‘sonar; 

( 

Vector3  *pool_normaI,  *ray_ncgalc_dircction.  ‘R. 
ini  inter,  any_inicr,  i,  g_s.  index,  nodejndex; 
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double  d.sonar.  t,  ND.  NO.  UO.  U 1 .  U2.  VO.  V 1 ,  V2; 

double  alpha,  beta,  distance,  retum_angle,  cunn_rangc,  shortest_distancc; 

Points  *P_inter, 

Points  •closest_pi; 

Points  *temp_pt; 

PJnter  =  VSNcw(0.0. 0.0, 0.0); 
closcstjjt  =  VSNew(0.0, 0.0, 0.0); 
temp_pt  =  V3New(0.0, 0.0, 0.0); 
inter  =  FALSE; 
any_inter  =  FALSE; 

ray  _negate_direc  lion  =  VSNew(0.0,0.0,0.0); 

R  =’vSNew(0.0,0.0,0.0); 

ray_negaie_direction->x  =  -ray_strucl->ray_dircciion->x; 
ray_negaie_direction->y  =  -ray_stnict->ray_direcuon->y; 
ray_negate_direction->2  =  -ray_struct->ray_dircction->z; 
index  =  ray_struct->ray_mairix_index; 
cum_rangc  =  0.0; 
shoriest_dislancc  =  9999.0; 

/*  Cycle  through  all  pool  polygons  in  iniiializc_virtual_pool.c  */ 
for  (i  =  0;  i  <  28;  ++i) 

{ 

/*  Check  for  proper  reflection  angle  (>  0  degrees)  */ 
rciurn_anglc  =  V3Dol(&pooltil.normal,  ray_ncgaie_dircciion); 
if  (return  anglou.O) 

I 

ND  =  VSDol(ray_struci'>ray_direction,  &pool[i].nonnal); 

NO  =  VSDot(ray_struct->ray_origin,  &pool[i].nornial); 

tcmp_pt->x  =  -pool[i].v0.x; 

tertip_pt->y  =  -pool[i].v0.y; 

tcmp_pi->z  =  -pool(i).v0.z; 

d_sonar  =  V3Doi(temp_pt,  &pooUi].nonnal); 

l  =  -1  ♦  ((d^sonar  +  NO)  /  ND);/*  Quick  range  check  */ 

if  ((t  >  0.0)  &&  (t  <=  2  *  MAX_SONAR_RANGE)  &&  (t  <  shortcst_distancc)) 

I 

/*  Calculate  the  point  of  intersection  */ 

P_intcr->x  =  ray_slruct->ray_origin->x  +  ray_struct->ray_direction->x  *  i; 
P_intcr->y  =  ray_slruct->ray_origin->y  +  ray_sirucl->ray_direclion->y  *  i; 
PJntcr->z  =  ray_struc'.->ray_origin->z  +  ray_sU'ucl->ray_dircciion->?.  *  l; 

UO  =  sclect_il(P_intcr->x.  P_intcr->y,  i)  - 
selecljl(pool[i).v0.x,  pool(il.v0.y,  i); 

VO  =  selcci_i2(P_inicr->y,  P_intcr->z,  i)  - 
select.  i2(poolti].v0.y.  poolli].v0.z,  i); 

inter  =  FALSE; 

Ul  =  select_il(pool[i].vl.x,pool(il.vLy,i) - 
select_il(pool[i].v0.x,  pooUiJ.vO.y,  i); 

U2  =  scIeclJl(pooIfi).v2.x,  pool[il.v2.y,  i)  - 
selcctjl(pool[i].v0.x,  pool[i].v0.y,  i); 
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VI  =  selccl_i2(pooUi].vl.y,  pooUi].vl.z,  i)  - 
seleclJ2(pool[il.v0.y,  pool[i].v0.z,  i); 
V2  =  select_i2(pooUi].v2.y,  pool[i].v2.z,  i)  - 
select_i2(pooUi].v0.y,  pool[il.v0.z,  i); 


/* 

*  Check  to  see  if  point  on  plane  is  within 

*  polygon 
*/ 

if(Ul  ==0) 

{ 

beta  =  U0/U2: 

if  ((beta  >=  0.0)  &&  (beta  <=  1.0)) 

{ 

alpha  =  (VO  -  beta  ♦V2)/ VI; 

inter  =  ((alpha  >=  0.0)  &&  (alpha  +  beta  <=  1.0)); 

} 

) 

else 

( 

beta  =  (V0*Ul  -  UO  *  VI)  /  (V2  *  U1  -  U2  *  VI); 
if  ((beta  >=  0.0)  &&  (beta  <=  1.0)) 

( 

alpha  =  (U0- beta  *U2)/U1; 

inter  =  ((alpha  >=  0.0)  &&  (alpha  +  beta  <=  1.0)); 

) 

) 

if  (inter)  /*  Point  was  in  poly  */ 

{ 

anyjnier  =  TRUE; 

distance  =  V3DisianccBetwccnTwoPoinis(P_inier,ray_slruci->ray_origin); 
if  (distance  <  shortesi_disiancc) 

{ 

shortest_distance  =  distance; 
closesi_pt->x  =  PJnler->x; 
closest_pl->y  =  P_inlcr->y; 
closest_pt->z  =  P_inter->z; 
node_index  =  i; 


) 

) 

if  (any_inter) 

{ 

R  =  compute_rc(lcction(ray_struct,  nodc_indcx); 
ray_struct->ray_mairix[index][0]  +=  shoriest^disiance; 
cum_range  =  ray_struct->ray_matrix(indcx](0]; 
ray_struct->ray_matrix[indcx]f  1]  =  c)oscst_pt->x; 
ray_slruct->ray_matrix[indcxl[2]  =  closest_pt->y; 
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ray_stnict->ray_matrix[index][31  =  closest_pt->z: 
ray_strucl->ray_matrix[indcx][4]  =  R->x; 
ray_strucl->ray_matrix[index][5]  =  R->y; 
ray_slruct->ray_mairix[index][6]  =  R->z; 

++ray_slruci->ray _mairi  x  [  index  ]  [7  ] ; 

if  (ray_stnict->ray_matrix[index][0]  <  2  *  MAX_SONAR_RANGE) 


/* 


*  reset  ray 

*  origin/direclion-recurse 
*/ 


{ 


++ray_slrucl->ray_matrix_index; 

ray_struct->ray_niatrix_index  =  ray_s!ruct->ray_inatrix_index  %  100; 
index  =  ray_siruct->ray_malrix_index; 
ray_struct->ray_matrix[index][0]  =  cum_range: 
ray_struct->ray_direction->x  =  R->x: 
ray_siruci->ray_direciion->y  =  R-'^y; 
ray_struct->ray_direction->z  =  R->z; 
ray_siruct->iay_origin->x  =  closesl_pi->x; 
ray_struct->ray_origin->y  =  closest_pi->y; 
ray_struct->ray_origin->z  =  closest_pt->z; 

ray_slruci->ray_mairix[indcxl[7]  +=  ray_slruct->ray_niatrix[indcx-l][7]; 
if  (ray_struct->ray_matrix(indcx][7]  <  4.0)  /*rccursion  depth*/ 
trace_ray(ray  .struct,  sonar); 

) 

) 

frec(closest_pt); 

free(P_inicr); 

free(temp_pt); 

free(ray_negaie_direciion); 


*compute_reflecuon0 

s(t**:f**  +  3(t*3(i****J»t:((;^t**»*%3fc******  +  ^()^(;Ji  *♦*♦***  +  *♦■***♦*»»**  j 

Vector3  *compute_reflection(ray_struct,i) 

SONAR.RAY  *ray_sU‘uct; 
int  i; 

{ 

Vector3  *L,  •two_N,  *R,  ♦temp; 
double  LdotN; 

L  =  V3Ncw(0.0, 0.0, 0.0); 
temp  =  V3New(0.0, 0.0, 0.0); 

R  =  V3New(0.0. 0.0, 0.0); 

L  =  V3Duplicaie(ray_struct->ray  .direction); 

L  =  V3Negate(L); 

LdotN  =  V3Dot(L,  &pool[i]. normal); 

L  =  V3DivideByScalar(L,  LdotN); 
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temp  *  V3Duplicale(&pool[i].normal); 
temp  =  V3MuUByScalar(lcmp,  2.0); 
temp  =  V3Sub(iemp,  L.  temp); 

R  =  V3DivideByScalar(  temp,  V3Length(tcmp)); 

free(L); 

free(temp); 

retum(R); 

} 

get_rangeO 

***»**»**•*»**»*»*»*  j 

void  get_range(sonar3uv,  ray  .struct) 

TRANSDUCER  *sonar; 

Sub_ptr  auv; 

SONAR.RAY  *ray_struct; 

{ 

double  min.range,  range_to_pl.  angle_bctwccn_rays,  d,  range_of_closcst_pl: 
ini  i,  j,  min.found; 

Vector3  ♦ray.negale.direction; 

LINE.SEGMENT  *sonar_linc; 

Point3  ♦closcsLpi; 

closest.pt  =  V3New(0.0, 0.0, 0.0); 
fange_of_closcsi_pi  =  9999.0; 
min.rangc  =  9999.0; 
min.found  =  FALSE; 

ray_negate_direction  =  V3New(0.0,0.0,0.0); 
sonarjine  =  &sonar->linc_data; 

for(i=0;  i  <  100;  ++i) 

{ 

if(ray  su-ucl->ray_matrix[i][7]  >=  I.O) 

( 

range_to_pt  =  V3DisianceBclweenTwoPoints(&ray_struct'>ray_matrix[iJl  I  j, 

sonar->position); 

if((rangc_lo_pi  +  ray_strucl->ray_malrix(i]iO])  <  2  *  MAX_SONAR_RANGE) 

{ 

min.rangc  =  (rangc_io_pi  +  ray_sirucl->ray_malrix[i][0])/2; 
if  ( min.range  <  range_of_closcst_pt ) 

( 

ray_negaie_dircclion->x  =  ray_sirucl->ray_malrix[i][l]  -  sonar->position->x; 
ray_negaie_direcuon->y  =  ray_strucl->ray_malrix[il[21  -  sonar->posiiion->y; 
ray_negale_direclion->z  =  ray_struct->ray_matrix[i][3]  -  sonar->position->z; 
anglc_bciwccn_rays  =  V3Dol(ray_ncgale_dircciion,  sonar->direclion); 

if  (anglc_bclwecn_rays  >  0.99619)  /*  cos  of  five  degrees  */ 

{ 

sonar->line_data.sonar_daia_pls[sonar->line_data.n_s][3]  =  min.rangc; 
sonar->line_data.sonar_dala_pts[sonar->linc_daia.n_sl[61  =  min.rangc; 
min.found  =  TRUE; 


101 


range_of_cIosest_pt  =  niin_range; 
closesi_pl->x  =  ray_slruct->ray_mairixfi][l]; 
closest_pi->y  =  ray_struct->ray_niairix[i][2]; 
closest  pl->z  =  rav_struct->rav_matrix(il[31; 

) 

} 

) 

) 

) 

if  (min_found) 

( 

pushmatrixQ; 

loadmatrix(auv->dyn.H_matrix); 
switch  (sonar->line_data.loc) 

{ 

case  1: 

translate(0.0,  -sonar->Unc_daia.sonar_data_pts[sonar->line_daia.n_s][3], 
-32.0): 
break; 

case  2: 

lranslate(sonar->line_daia.sonar_data_pts[sonar->line_data.n_sj[3],0.0, 
-32.0 ); 
break; 

case  3: 

iranslate(-sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3],0.0, 
-32.0 ); 
break; 

case  4: 

iranslaic(0.0, 0.0, 

-sonar->line_data.sonar_data_pls[sonar->line_dala.n_s][3]  -  32.0 ); 
auv->front_range  = 

sonar->Iine_data.sonar_data_pts[sonar->line_data.n_s]  [3] ; 
break; 

) 


gclmalrix(sonar->sonar_malrix); 

popmatrixO; 

if(match_modcl(sonar_line)  ==  TRUE) 

{ 

sonar->line_data.sonar_data_pis[sonar->line_data.n_s][3)  =  1.0; 
++sonar_l  inc  ->n_s; 

sonar_line->n_s  =  sonar_linc->n_s  %  100; 

1 

else 

{ 

if(sonarJinc->rangc_pt  <  2) 

{ 
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++sonar_line->n_s; 

sonar_linc->n_s  =  sonarJinc->n_s  %  100; 

} 

++sonar_Une->range_pl; 

) 

sonar->linc_data.sonar_daia_pu[sonar->line_daia.n_s][0]  =  sonar->sonar_matrix[3][0]; 
sonar->line_data.sonar_data_pts[sonar->line_(lata.n_s][l]  =  sonar->sonar_matrix[3][l]; 
sonar->line_data.sonar_daia_pls[sonar->Iine_data.n_s][2]  =  sonar->sonar_matrix[3][2]; 
sonar->linc_data.sonar_data_pts[sonar->Iinc_daia.n_s][4]  =  auv->dyn.hcading; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][5]  =  auv->dyn.H_matrix[3][  1  ]; 

if  (sonar_line->range_pl  >  1) 

{ 

if(sonar->line_data.sonar_data_pu[sonar->line_data.n_s][3]  ==  1.0) 

( 

if(sonarJine->range_pt  >  M1N_N0_PTS) 

{ 

end_Iinc_segment(sonar_Iine); 
rescl_linc(sonar_linc,closcsi_pl,  min_rangc,  auv); 
sonarJine->sonar_data_pis[sonar_line->n_s][3]  =  inin_range; 

) 

else 

{ 

resctJine(sonarJine,  cIosesl_pi,  min_rangc,  auv); 
sonarJinc->sonar_daia_pls[sonar  linc->n_s][3]  =  min_rangc; 

) 

1 

else 


chcck_coursc(sonar_Unc,  auv); 
check_depth(sonarJine,  auv); 


if((sonarJine->tooJong  ==  TRUE)  11  (sonar_line->course_change  =  TRUE) 

II  (sonar_line->depih_change  ==  TRUE)  II  (sonar_line->loo_far_apart  ==  TRUE)) 


if(sonar_linc->rangc_pl  >  MIN_NO_PTS) 

( 

cnd_linc_segment(sonar_Iinc); 
resei_line(sonarJine,closest_pi,  min_range,  auv); 
sonarJine->sonar_daia_pts[sonar_line->n_s][3]  =  min_rangc; 
line_segmenUnit(sonar); 

) 

else 

{ 

reselJine(sonarJine,  closest_pt,  min_range,  auv); 
sonar_linc->sonar_data_pts[sonar_Iine->n_s][3I  =  min_rangc; 
line_segmeniJnii(sonar); 

) 


else 
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{ 


++sonar_line->rangc_pl; 
if  (sonarJine->loc  !=  5) 

if  (sonar_line->rangc_pt  <=  MIN_NO_PTS) 

( 

line_segment_init(sonar); 

) 

if  (sonar_line->range_pt  >  MIN_NO_PTS) 
linc_scg_compulc(sonar); 

) 

] 

) 

free(ray_negate_direclion) ; 
free(closesl_pt); 

) 

I  ^^Ht:^i^i^i^iif^^im***************************<lf:************** 

match_modeIO 

int  match_model(sonar_line) 

LINE_SEGMENT  *sonar_linc; 

{ 

int  match,  i; 
match  =  0; 

for(i  =  0;  i  <  28;  ++i) 

( 

if((fabs(sonar_line->r_sonar  -  p<x)l[i].r)  <=  MAX_DELTA_R)  && 

(fabs(sonar  linc->iheta_sonar  -  pool[i].thela)  <=  MAX_DELTA_THETA)) 

{ 

match  =  1; 


) 

retum(match); 

) 

j  :^3tt**4(**4<**«***^*’l<*3^4<*****4t*4[*^^*****«****4(^4i*««*4'**3ti* 

repIay_sonar() 

»*«**4>***«*»»*********»4>************4>***************  ! 

void  replay _sonar_data(sonar,  auv) 

TRANSDUCER  *sonar; 

Sub_ptr  auv; 

{ 

double  SIN_5,  raw_mg; 

SIN_5  =  .087155; 

raw_mg  =  auv->dyn.commanded_posture[auv->dyn.posture_no][14]; 
pushmatrixO; 

loadmatrix(auv->dyn.H_mau‘ix); 
rotate(-(Angle)auv->dyn.pos_changc[51,  ‘y’); 
rotatef  (Angle)auv->dyn.pos_change[4],  ‘x’); 
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rotate(-(Angle)auv->dyn.pos_changc[3],  ‘z’); 

switch  (sonar->line_data.loc) 

1 

case  1: 

translaie(0.0.  -raw_mg  *  CONVERT_TO_INCHES,  -32.0); 
break; 
case  2: 

translalc(raw_mg  *  CONVERT_TOJNCHES,0.0,  -32.0 ); 
break; 
case  3: 

if(fabs(auv->dyn.cominanded_posture[auv->dyn.posture_no][9])  >  0.04357) 
raw_rng  +=  raw_mg  •  S1N_5; 
translate(-raw_rng  *  CONVERT_TO_INCHES,0.0,  -32.0); 
break; 

case  4; 

lranslate(  0.0, 0.0,  -raw_rng  *  CONVERT_TO_INCHES  -  32.0); 
break; 

] 

gettnatrix(sonar->sonar_matrix); 

popmatrixO; 


sonar->linc_data.sonar_daia_pls[sonar->linc_daia.n_s](0]  =  sonar->sonar_nialrix[3][0]; 
sonar->linc_data.sonar_data_pts[sonar->linc_data.n_s]l  1  ]  =  sonar->sonar_mairix[3][  1  ]; 
sonar->line_data.sonar_daia_pts(sonar->line_data.n_s](2]  =  sonar->sonar_matrix[3][2]; 
sonar->line_data.sonar_data_pis[sonar->linc_daia.n_s][3]  =  auv->dyn.H_matrix[3][0]; 
sonar->line_daia.sonar_data_pts[sonar->line_data.n_s][4]  =  auv->dyn.H_matrix[3][l]; 
sor>ar->line_data.sonar_data_pis[sonar->linc_daia.n_s][5]  =  auv->dyn.H_mairix[3][2]; 
++sonar->line_daia.n_s; 

sonar->line_dau.n_s  =  sonar->line_data.n_s  %  1500; 

} 

Vector  library  routines  from  “GRAPHICS  GEMS”,  edited  by  Glassner  [Ref.  26] 

*********i|i»**«*«***«4i***  ***•:»**»****»*  *41**  *****************************  :ti***4‘>l<***»***^ 

/*  return  the  dot  product  of  vectors  a  and  b  */ 
double  V3Dot(a,  b) 

Vector3  *a.  *b; 

{ 

return  ((a->x  •  b->x)  +  (a->y  *  b->y)  +  (a->z  *  b->z)); 

] 

Vector3  ♦V3Negate(v) 

Vector3  ’v; 

{ 

v->x  =  -v->x; 
v->y  =  -v->y; 
v->z  =  -v->z; 
return  (v); 

) 
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/*  return  vector  sum  c  =  a  +  b  */ 

Vectors  *V3Add(a,  b,  c) 

Vectors  *a,  *b,  *c; 

( 

c->x  =  a->x  +  b->x; 
c->y  =  a->y  +  b->y; 
c->z  =  a->z  +  b->z; 
rctum(c); 

) 

/*  create,  initialize  and  return  a  new  vector  */ 
Vectors  *VSNew(a,  b,  c) 
double  a,  b,  c; 

{ 

Vectors  *v  =  NEWTYPE(Vcctor3); 

v->x  =  a; 
v->y  =  b; 
v->z  =  c; 
return  (v); 

} 


/*  scales  the  input  vector  to  tlic  new  length  and  returns  it  */ 
Vectors  *VSMultByScalar(v,  scalar) 

Vectors  *v; 
double  scalar; 

{ 

v->x  *=  scalar; 
v->y  *=  scalar; 
v->z  *=  scalar; 
retum(v); 

) 


/*  return  vector  difference  oc  =  a  -  b  */ 

Vectors  *VSSub(a,  b,  c) 

Vectors  *a,  *b,  *c; 

{ 

c->x  =  a->x  -  b->x; 
c->y  =  a->y  -  b->y; 
c->z  =  a->z  -  b->/.: 
retum(c); 

) 

/*  divides  the  vector  by  a  scalar  and  returns  it  */ 
Vectors  *VSDivideByScalar(v,  scalar) 

Vectors  *v; 
double  scalar; 

{ 

if  (scalar  !=  0.0) 

I 
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v->x  =  v->x/scalar; 
v->y  =  v->y/scalar; 
v->z  =  v->z/scalar; 
relum(v); 

) 

1 

/*  returns  squared  length  of  input  vector  */ 
double  V3SquaredLcnglh(a) 

Vcctor3  *a; 

{ 

retum(SQR(a->x)  +  SQR(a->y)  +  SQR(a->z)); 


/*  returns  length  of  input  vector  *! 
double  V3Lengih(a) 

Vector3  *a; 

( 

rctum(sqrt('V3Squarcdl.cnglh(a'>’)'): 

} 


/*  normalizes  the  input  vector  and  returns  it  */ 
Vector3  *V3Normalize(v) 

Vector3  *v; 

( 

double  len  =  V3Length(v); 
if(len!=0.0) 


v->x  /=  len; 
v->y  /=  len; 
v->z  /=  len; 
retum(v); 


/*  create,  initialize,  and  return  a  new  vector  *! 
Vector3  *V3Duplicatc(a) 

Vcctor3  *a; 

{ 

Vcctor3  *v  =  NEWTYPE(Vcctor3); 

v->x  =  a->x; 

v->y  =  a->y; 

v->z  =  a->z; 

relum(v); 

) 

/*  return  the  distance  bctwqeen  two  points  */ 
double  V3DistanccBctwecnTwoPoints(a,  b) 
Point3  "'a,  *b; 

( 

double  dx  =  a->x  -  b->x; 
double  dy  =  a->y  -  b->y; 
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double  dz  =  a->z  -  b->z; 
rctum(sqrt(SQR(dx)  +  SQR(dy)  +  SQR(dz))); 

} 

void  V3MuliMalrixByPoinn^sonarj 
TRANSDUCER  *sonar; 

{ 

Vectors  •iemp_pi: 

temp_pt  =  V3New(0.0, 0.0, 0.0); 

temp_pt->x  =  sonar->sonar_matrix[0][0]  *  sonar->direclion->x  + 
sonar->sonar_matrix[0][l]  *  sonar->direction->y  + 
sonar->sonar_matrix[0][2]  *  sonar->direction->z; 
lemp_pi->y  =  sonar->sonar_matrix[lJ{0]  *  sonar->direciion->x  + 
sonar->sonar_matrix[l][l]  *  sonar->direction->y  + 
sonar->sonar_matrix[l][2]  *  sonar->dircction->z; 
lemp_pi->z  =  sonar->sonar_mauix[2]t0]  *  sonar->dirccuon->x  + 
sonar->sonar_matrixf21fll  *  sonar->direction->y  + 
sonar->sonar_malrix[2][2J  sonar->dircciion->/.; 
sonar->direction->x  =  temp_pt->x; 
sonar->dircclion->y  =  icmp_pt->y; 
sonar- >dircction->/.  =  icmp_pi->z; 

) 

This  package  takes  individual  range  points  from  sonar_bay-c  and 

computes  a  least-squares  fit  line  for  the  points.  The  line  is  ended 

for  various  reasons  and  a  plane  is  fit  to  the  line,  based  on  the 

computed  end  points  and  the  width  is  determined  by  the  range  from  the 

sonar.  Points  and  planes  are  passed  back  to  sonar_bay.c  for  plotting 

*♦******♦»*♦**•*»»»*»*»♦*»♦♦»***»*»***»»»****»»»**»»»*»**♦****»**»*****»»*»****»»»» 


#include  <stdio.h> 

#include  <math.h> 

^include  “auv.h” 

#include  “sonar.h” 

I  *m*^*m******m*iit***^t***i^***m******^******************m 

line_segmentjnit() 

Initialize  a  line  segment  and  its  associated  variablcs/ilags 
Called  from  sonar_range()  in  sonar_bay.c. 

4t)|i*4i4i*)^4<*4i4i4(*4[4>***)t<4t4(4r***3ti4(******^******4i***4<*4>*4i4(>t(*4>  j 

line_segment_init(sonar) 

TRANSDUCER  *sonar; 

{ 

LINE_SEGMENT  *line; 

line  =  &sonar->line_data; 
if  ((Iinc->loc  !=  1)  &&  (line->loc  !=  5)) 
convert_coords(line); 
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/*  Read  in  first  points  to  establish  initial  line  segment  */ 
line->line_lengih  =  0.0; 
line->bad_pt  =  FALSE: 

f*  accumulate  variables  */ 
line->sgmx  +=  Iine->sonar_daia_pts[line->n_s][0]; 
line->sgmy  +=  line->sonar_daia_pls[line->n_s][l]; 
linc->sgmx2  +=  SQR(linc->sonar_daia_pLs[linc->n_s][01); 
line->sgmy2  +=  SQR(line->sonar_data_pts[linc->n_s][l]); 

line->sgmxy  +=  Iine->sonar_daia_pts[line->n_s][0]  ♦  line->sonar_data_pts[line->n_s][l]; 
line->end_pl_no  =  line->n_s; 

/*  Update  the  counters  */ 
if  (line->n_s  =  99) 
line->n_s  =  0; 
else 

++linc->n_s; 

if  (line->rangc_pt  ==  0) 

linc->start_pt_no  =  linc->n_s; 

++line->i_s:/*  current  line  segment  point  counter  */ 


if  (line->rangc_pt  ==  \f  IN_NO_PTS)/*  use  x  data  points  for  first  segment  •/ 

{ 

/•  Calculate  first  line  segment  values  */ 
line->mux  =  line->sgmx  /  line->i_s; 
line->muy  =  line->sgmy  /  linc->i_s; 

iinc->muxx  =  line->sgmx2  -  (line->sgmx  ♦  line->sgmx)/  linc->i_s; 
line->muyy  =  line->sgmy2  -  (line->sgmy  *  line->sgmy)  /  line->Ls; 
line->muxy  =  line->sgmxy  -  (line->sgmx  *  linc->sgmy)  /  line->i_s; 

line->end_pt_no  =  line->n_s  -  1; 
if  (line->end_pt_no  <  0) 

line->end_pt_no  =  line->end_pt_no  +  100; 

line->theta_sonar  =  (atan2(-2.0  *  line->muxy,  (line->muyy  -  line->muxx))}  /  2.0; 
linc->r_sonar  =  line->mux  *  cos(line->thcta_sonar)  +  line->muy  *  sin(linc->ihcta_sonar); 

for  (line->j_s  =  0;  line->j_s  <  MIN_NO_PTS;  ++linc->j_s) 

I 

line->j_s  =  (line->j_s  +  line->start_pt_no)  %  100; 

line->sgm_dplta_sq  +=  SQRnine->sonar_data_pts[line->j_s][0]  -  line->mux) 

*  SQR(cos(line->lheta_sonar)); 

line->sgm_delta_sq +=  SQR(line->sonar_data_pls[line->j_s][l]  -  line->muy) 

*  SQR(sin(linc->theta_sonar)); 

line->sgm_delta_sq  +=  2.0  *  Oine->sonar_daia_pls[line->j_s][0]  -  line->mux) 

*  (line->sonar_data_pisiUne->j_sl[l]  -  line->muy) 

*  cos(Iine->lhcta_sonar)  •  sin(linc->theia_sonar); 

) 
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y*»»***4i******4i******»*»4'«4<4<******4>*»»4' **»*****»»»»*** 

linc_scg_coitipuicO 

Read  in  subsequent  data  points,  after  a  line  segment  has  been 
initialized  and  more  range  values  are  obtained 

»********************:»««»4i***»**»*»****»»******»4<**4>^ 


line_seg_compute(sonar) 

TRANSDUCER  ♦sonar; 


{ 

LINE_SEGMENT  ‘line; 

/*  line  &=  sonar->linc_daia;  */ 
line  =  &sonar->line_data; 

if  ((line->loc  !=  I)  &&  (line->loc  !=  5)) 
convcrt_coords(lip.c); 


/*  Calculate  test  values  •/ 

line->sigma  =  linc->sgm_dclia_sq  /  linc->i_s; 


/•  Test  new  point  for  linearity  fit  */ 

line->dcltajinc  =  linc->sonar_data_pLs[line->n_s](0)  *  cos(line->thcta_sonar) 

+  linc->sonar_daui_pts[linc->n_sl(ll  *  sin(line->tlicia_sonar) 

-  linc->r_sonar; 

if  ((fabs(line->dclta_line)  <  (line->sigma  *  cl))  II  (fabs(linc->dclia_linc)  <  c2)) 

{ 

line->sgmx  +=  Iinc->sonar_daia_pts[linc->n_s](0]; 
line->sgmy  +=  linc->sonar_data_pts[line->n_s]nj; 
line->sgmx2  +=  SQR(linc->sonar_data_pts(line->n_s][0]); 
linc->sgmy2  +=  SQR(linc->sonar_data_pLs(linc->n_sJ[  1  ]); 

line->sgmxy  +=  line->sonar_data_pts[line->n_s]lO]  ♦  linc->sonar_daia_pts[linc->n_s][l]; 

line->mux  =  line->sgmx  /  (linc->i_s  +  1); 

line->muy  =  line->sgmy  /  (line->i_s  +  1); 

linc->muxx  =  line->sgmx2  -  SQR(linc->sgmx)  /  (line->i_s  +  1); 

line->muyy  =  !inc->sgmy2  -  SQR(line->sgmy)/(line->i_s  +  1); 

line->muxy  =  linc->sgmxy  -  (line->sgmx  *  linc->sgmy)  /  (linc->Ls  +  1); 

/♦  calculate  ellipse  values  */ 

line->m_major  =  (line->muxx  +  line->muyy)  /  2.0  -  sqn((line->muyy  -  line->muxx) 

•  (linc->muyy  -  line->muxx)  /  4.0  +  SQR(line->muxy)); 
line->m_minor  =  (linc->muxx  +  line->muyy)  /  2.0  +  sqrt((line->muyy  -  line->muxx) 

♦  (line->muyy  -  line->muxx)  /4.0  +  SQR(linc->muxy)); 
line->d_major  =  4.0  *  sqrt(fabsOine->m_minor/(line->i_s  +  I))); 
line->d_minor  =  4.0  *  sqrt(fabs(linc->m_major  /  (linc->i_s  +  1))); 
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/*  Test  new  point  for  ellipse  line->lhinncss  */ 
if  ((line->d_minor  /  line->d_major)  <  c3) 


f 

linc->end_pi_no  =  linc->n_s;/*  update  end  point  */ 

/* 

*  update  line  segment  parameters  to  include  new 

•  point 

*1 

line->thela_sonar  =  (alan2(-2.0  *  linc->muxy,  (linc->muyy  -  linc->muxx)))  /  2.0; 
line->r_sonar  =  line->mux  *  cos(linc->thcta_sonar)  +  linc->muy 

*  sin(linc->thcta_sonar); 

linc->sgm_dclta_sq  ■•■=  2.0  •  (Iine->sonar_data_pis[line'>n_s][0]  -  line->mux) 

*  (line->sonar_data_ptsllinc->n_sl[l]  -  linc->muy) 

*  cos(line->theta_sonar)  *  sin(linc->theta_sonar); 
linc->sgm_delta_sq  +=  SQR(linc->sonar_data_pLs[linc->n_slfl)  -  linc->muy') 

*  SQR(sm(lme->tlieta_sonar)); 

line->sgm_delta_sq  +=  SQR(linc->sonar_data_pts(linc->n_sl(01  -  linc->mux) 

*  SQR(cos(line->thcui_sonar)); 

if  (linc->n_s  ==  99) 
tmc->n_s  =  0; 
else 

++linc->n_s; 

++line->i_s; 

linc->dclla_x  » Iine->sonar_data_ptslline->start_pt_nol[0) 

- 1  inc  ->sonar_dau_pts(  1  inc->cnd_pt_no]  [  0] ; 
linc->dclui_y  =  linc->sonar_daia_pts|line->siari_pt_no][l) 

-  linc->sonar_daia _ptsllinc->cnd_pt_no]l  1]; 
linc->linc_lcngth  =  sqn(SQR(line->delta_x)  +  SQR(line->dclta_y) 

+  SQR(linc->sonar_data_pisllinc->siart_pt_no]131 

-  linc->sonar_data_pts(linc->cnd_pt_no][3))); 

/•if  (line->lincjcngth  >  M  AX_LINE_LENGTH) 

( 

line->too_long  =  TRUE; 
cnd_linc_scgmcnt(linc); 

) 

chcck_for_tcrmination(linc):*/ 

)  el.se 

( 

line->bad_pt  =  TRUE; 

cnd_line_scgmcnl(linc); 


}  el.se 

{ 

line->bad_pt  =  TRUE; 
end_l  inc_scgmcnUl  inc); 

1 
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end_linc_scginemO 

Wrap  up  a  line  segment  if  bad  data  pi,  course  change,  depth  change, 
or  segment  max  length  reached. 


end_line_segment(linc) 

LINE_SEGMENT  *linc; 


int  i; 

double  line_anglc; 

if  ((linc->bad_pt  ==  TRUE}) 

{ 

/*  start  now  lino  .sogmont  ’/ 
linc->sgmx  =  linc->sonar_data_pLs[line->n_sl(01; 
linc->sgmy  =  linc->sonar_data_pLs[linc->n_s][ll; 
linc->.sgmx2  =  SQR(linc->sonar_data_pts(linc->n_s]10]); 
line->sgmy2  =  SQR(line->sonar_data_pis(line->n_s][l]); 

linc->sgm.\y  =  Iinc->sonar_tlata_pts(linc->n_,s|[0)  *  linc->sonar_dala_pi.sllino->n_s]i  1 1; 

linc->i_.s  =  1: 

linc->sigma  =  0; 

linc->sgm_dclia_sq  =  0; 

linc->rangc_pl  =  1; 

)  else 

linc->rango_pt  =  0; 


/•  close  out  old  segment,  convert  radius  to  positive  value  first  */ 
if  (line->r_sonar  <  0) 

I 

linc->ihcia_sonar  =  180  ♦  DEG_TO_RAD  +  linc->lheia_sonar; 
linc->r_sonar  =  -1  ♦  linc->r_sonar; 

1 

/*  determine  start  and  end  points  on  the  computed  line  segment  */ 
linc->siart_pi_x  =  linc->sonar_dala_pLs[linc->slart_pt_no]|0); 
line->start_pt_y  =  linc->sonar_daia_pts[line->siart_pi_no][l]; 
linc->cnd_pt_x  =  line->sonar_daia_plsflinc->cnd_pl_no](01; 
line->end_pi_y  =  linc->sonar_daia_pls[linc->end_pl_no][l]; 
line->dclia_linc  =  linc->stari_pi_x  *  cos(Iine->thcta_sonar)  +  linc->siari_pi_y 
*  sin(line->lheta_sonar)  -  fabs(line->r_sonar); 
linc->siarl_pi_x  =  linc->slarl_pt_x  -  (line->dclta_linc  *  cos(linc->thcta_sonar)); 
line->start_pt_y  =  line->stan_pi_y  -  (line->delia_line  •  sin(line->lheia_sonar)); 
linc->dcllajinc  =  linc->cnd_pt_x  *  cos(Iinc->lhcui_sonar)  +  linc->cnd_pl_y  *  sin(Iinc->lhcta_sonar} 
-  fabs(linc->r_sonar): 

l..ic->cnd_pi_x  =  linc->cnd_pi_x  -  (linc->dcllajinc  *  cos(line->lhela_sonar)); 
linc->cnd.  pi_y  =  linc->cnd_pt_y  -  flinc->dclta_linc  *  sin(line->lhcia_sonar)): 
line->dclia_x  =  linc->sian_pi_x  -  line->cnd_pl_x; 
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line->delia_y  =  line->start_pt_y  -  line->end_pt_y; 
line->line_length  =  sqrt(SQR(line->delta_x)  +  SQR(line->delta_y) 

+  SQR(line->sonar_daia_pls[line->siart_pi_no][2] 

-  line->sonar_daia_pts[line->end_pt_nol[2])); 

if  (line->loc  =  1)/*  Bottom  sonar  ♦/ 

{ 

line_angle  =  atan2(line->sonar_daia_ptsIline->siart_pt_no}[2] 

-  line->sonar_data_pts[line->cnd_pt_no][2], 
line->sonar_data_pts[line->start_pt_noKO] 

-  Iine->sonar_data_pts[line->end_pt_no][0]); 

Une_angle  =  line_anglc  -  PIOVER2; 

)  else 

( 

line_angle  =  atan2(line->sonar_data_pts[Hne->start_pt_no][l] 

-  line->sonar_data_pts[line->end_pt_no][  1  ], 
sqrt(SQR(line->sonar_data_pts[line->start_pt_no][0] 

- 1  ine->sonar_daia_ptsriine->end_pt_nol  [0] ) 

+  SQR(line->sonar_data_pts[line->start_pi_no][2J 

-  line->sonar_data_pts[line->end_pt_no]  [2]))); 

} 

if  ((line->loc  ==  1)  11  (line->loc  ==  5))/*  Bottom  sonar  */ 

{ 

offsctlc  =  linc->sonar_data_pts[linc->su>.rt_pt_no][3]  *  0.0871548  *  cos(linc_angIc); 
offsetls  =  linc->sonar_daia_pts[linc->siari_pt_no][3]  *  0.0871548  *  sin(line_anglc); 
offset2c  =  line->sonar_data_pis[line->end_pt_no][3]  *  0.0871548  *  cos(linc_anglc); 
offset2s  =  line->sonar_data_pts[line->end_pt_no][3]  •  0.0871548  •  sin(line_anglc); 

)  else 

{ 

offsctlc  =  line->sonar_daia_pis[linc->siart_pLno][3]  *  0.0871548  *  cos(linc_anglc): 
offsetls  =  line->sonar_data_pts[line->siart_pt_no][3]  *  0.0871548  *  sin(line_angle); 
ofrset2c  =  line->sonar_data_pts[line->end_pt_no][3]  *  0.0871548  *  cos(line_angle); 
offset2s  =  line->sonar_data_pis[line->end_pt_no][3]  *  0.0871548  *  sin(line_anglc); 

} 

if  ((line->line_length  >24.0)  &&  (line->loc  !=  4)) 

{ 

set_plane_ptr(line); 
if  (line->n_plane  <  99) 

++line->n_plane; 

else 

line->n_plane  =  0; 


line->n_s  =  0; 

Iine->start_pl_no  =  0; 
line->end_pt_no  =  0; 

for  (i  =  0;  i  <  100;  ++i) 

line->sonar_data_pts[i][3]  =  0.0; 

if(offsetlc  >  48.0)  /*  limit  width  of  planes  to  four  feel  */ 
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offsetlc  =  48.0; 
if(offsells  >  48.0) 
offsells  =  48.0; 
if(offset2c  >  48.0) 
offsetlc  =  48.0; 
if(offset2s  >  48.0) 
offsetls  =  48.0; 


convert_coordsO 

convert_coords(line) 

LINE.SEGMENT  *line; 


{ 

double  lemp_val: 

/* 

*  store  2  data  into  y  var,  y  into  z  for  bottom  sonar 
V 

temp_val  =  Iinc->sonar_data_pts[linc->n_s][2]; 
line->sonar_daia_ptsflinc->n_s]f21  =  Iinc->sonar_data_ptsfIinc->n_s]f  1  ]: 
line->sonar_data_pts[line->n_s][l]  =  temp_val; 

) 


check_for_lenTnination() 

check_for_tennination(linc) 

LINE.SEGMENT  *linc; 

{ 

if  (sqn(SQR(Hne->sonar_data_pls(Iine->stan_pi_no]  [0] 

-  Iine->sonar_data_pts[line->end_pt_no][0]) 

+  SQR(line->sonar_data_plslline->start_pt_noJl  1] 

-  line->sonar_data_pts[line->end_pt_no](l]) 

+  SQR(line->sonar_data_pts[line->siart_pt_no]l2] 

-  Iine->sonar_data_pts(line->end_pi_no)[2])) 

>  MAX_LINE_LENGTH) 

{ 

line->end_pi_no  =  line->end_pt_no  - 1; 

Iine->too_long  =  TRUE; 

)  % 

if  ((line->loc  ==  1)  II  {line->loc  ==  4)) 

{ 

if  (fabs(line->sonar_data_pis[line->stan_pt_no][4] 

-  line->sonar_daia_ptsfline->end_pt_no]t41) 

>  MAX_COURSE_CHANGE) 
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( 

line->course_change  =  TRUE; 

) 

)  else 

{ 

if  (fabs(line->sonar_data_pts[line->start_pt_no]  [4] 
-  line->sonar_data_plsfline->cnd_pt_no][4]) 
>  MAX_DEPTH_CHANGE) 

I 

line->depth_change  =  TRUE; 


) 

} 

if  ((line->tooJong)  II  (line->course_change)  II  (line->depth_change)) 
end_line_segmcnl(line); 

) 

I  ********  ***»****r***************r  ******** 

set_plane_ptr() 

Store  the  plane  data  points  into  the  array  for  display. 
set_plane_pir(line) 

LINE_SEGMENT»line: 

{ 

inti; 

double  line.angle; 

if  ((line->loc  ==  1)  II  (linc->loc  ==  5)) 

{ 

Iine->plane_pts[line->n_plane][0][0]  =  line->sonar_data_pts[line->start_pt_no][0]  +  offsetlc; 
line->plane_pts[line->n_plane]  [01[  1]  =  linc->sonar_data_pts[line->start_pt_no]  [  1  ] ; 
Iine->plane_pts[line->n_plane][0][2]  =  line->sonar_data_pls[Iine->siartj)t_no][2]  +  offsetls; 
Iine->plane_pts[line->n_plane][l][0]  =  Iine->sonar_data_pts[line->start_pt_no][0]  -  offsetlc; 

Iine->plane_pts[line->n_plane][l][l]  =  linc->sonar_dala_pts[linc->start_pt_no][lJ; 
line->plane_pts[line->n_plane][l}[2]  =  line->sonar_data_pts[line->start_pt_no][2]  -  offsetls; 
line->plane_pts[line->n_planc][2][01  =  Iine->sonar_daia_pts[linc->end_pt_no][0]  +  offsci2c; 

line->plane_pts[line->n_planell2][ll  =line->sonar_data_pts[line->end_pt_no]fl]; 
line->plane_pts[line->n_plane]f2][2]  =  line->sonar_data_pts[line->end_pt_no][2]  +  offset2s; 
line->plane_pts[linc->n_plane][3][01  =  line->sonar_data_pts[line->end_pt_no][0]  -  offsct2c; 

line->plane_ptsninc->n_plane]r3][l]  =  linc->sonar_data_pls[line->end_pt_no]fl]; 
line->plane_pls[line->n_plane][3][21  =  line->sonar_daia_pts[line->end_pt_no][2]  -  offset2s; 

)  else 

{ 

line->pIane_pts[line->n_plane][0][0]  =  line->sonar_data_pts[line->start_pt_no][0] 

+  offsetlc  *  cosOine->lheta_sonarX 

Iine->plane_pts[line->n_plane][0][ll  =  !ine->sonar_data_pts[line->start_pt_no][2]  +  offsetls 
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offsetlc; 

Iine->plane_pts[line->n_plane]f0]f21  =  line->sonar_data_pts[Iine->stari_pi_no]  [  1  ] ; 
line->plane_pts[line->n_plane][  1][0]  =  Iine->sonar_data_pts[line->start_pi_no][0] 
+  offsetlc  *  cosGine->theta_sonar); 

linc->plane_ptslline->n_planc][l][l]  =  line->sonar_daia_pis[linc->sian_pt_noJI2J 

-  offset!  s  -  offsetlc; 

line->plane_pts0ine->n_plane][l][2]  =  line->sonar_data_pts[line->start_pt_no][l]: 
line->plane_pts[line->n_plane][2][01  =  Iine->sonar_data_pts[line->end_pt_no][0] 

+  offsetlc  *  cosOine->theta_sonar); 

line->plane_pts[line->n_plane]  [2]  [  1]  =  line->sonar_data_pts[line->end_pt_no]  [2] 

+  offset2s  +  offsei2c; 

line->plane_pts[line->n_plane][2][2]  =  line->sonar_daia_pts[Hne->end_pt_no][l]; 
line->plane_pts[Une->n_plane][31[0]  =  Iine->sonar_data_pts[line->end_pt_no][0] 

+  offsetlc  *  cosOine->theta_sonar); 

line->plane_pts[line->n_plane][3][l]  =  Iine->sonar_data_pts[line->end_pt_no][2] 

-  offset2s  -  offset2c; 

line->plane_pts[line->n_plane][3][2]  =  line->sonar_data_pts[line->end_pt_no][l]; 
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